diff --git a/.gitignore b/.gitignore index da442c20b7..3f6f5456a1 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,7 @@ # /conf/ /conf/conf.xml /conf/conf.xml.20* +/conf/conf_personal.xml.20* /conf/control_panel.xml /conf/%gconf.xml /conf/maps_data/* diff --git a/Makefile b/Makefile index 39e02ab114..b46b46b178 100644 --- a/Makefile +++ b/Makefile @@ -113,7 +113,7 @@ update_google_version: conf: conf/conf.xml conf/control_panel.xml conf/maps.xml -conf/%.xml :conf/%.xml.example +conf/%.xml :conf/%_example.xml [ -L $@ ] || [ -f $@ ] || cp $< $@ @@ -248,6 +248,15 @@ paparazzi: chmod a+x $@ +# +# doxygen html documentation +# +dox: + $(Q)PAPARAZZI_HOME=$(PAPARAZZI_HOME) sw/tools/doxygen_gen/gen_modules_doc.py -pv + @echo "Generationg doxygen html documentation in doc/generated/html" + $(Q)( cat Doxyfile ; echo "PROJECT_NUMBER=$(./paparazzi_version)"; echo "QUIET=YES") | doxygen - + @echo "Done. Open doc/generated/html/index.html in your browser to view it." + # # Cleaning # @@ -285,7 +294,7 @@ ab_clean: replace_current_conf_xml: test conf/conf.xml && mv conf/conf.xml conf/conf.xml.backup.$(BUILD_DATETIME) - cp conf/tests_conf.xml conf/conf.xml + cp conf/conf_tests.xml conf/conf.xml restore_conf_xml: test conf/conf.xml.backup.$(BUILD_DATETIME) && mv conf/conf.xml.backup.$(BUILD_DATETIME) conf/conf.xml @@ -296,7 +305,7 @@ run_tests: test: all replace_current_conf_xml run_tests restore_conf_xml -.PHONY: all print_build_version update_google_version ground_segment ground_segment.opt \ +.PHONY: all print_build_version update_google_version dox ground_segment ground_segment.opt \ subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt tools\ static sim_static lpctools commands \ clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \ diff --git a/Makefile.ac b/Makefile.ac index 90e2b5e3d0..9939768bcd 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -64,10 +64,19 @@ Q=@ # ifeq ($(MAKECMDGOALS),all_ac_h) -include $(MAKEFILE_AC) + +ifdef PERIODIC_FREQUENCY +# telemetry and module periodic frequency default to PERIODIC_FREQUENCY +TELEMETRY_FREQUENCY ?= $(PERIODIC_FREQUENCY) +DEFAULT_MODULES_FREQUENCY = $(PERIODIC_FREQUENCY) +else +$(warning Info: PERIODIC_FREQUENCY not configured, defaulting to 60Hz for modules and telemetry) +TELEMETRY_FREQUENCY ?= 60 +DEFAULT_MODULES_FREQUENCY = 60 +endif + endif -# telemetry periodic frequency defaults to 60Hz -TELEMETRY_FREQUENCY ?= 60 init: @[ -d $(PAPARAZZI_HOME) ] || (echo "Copying config example in your $(PAPARAZZI_HOME) directory"; mkdir -p $(PAPARAZZI_HOME); cp -a conf $(PAPARAZZI_HOME); cp -a data $(PAPARAZZI_HOME); mkdir -p $(PAPARAZZI_HOME)/var/maps; mkdir -p $(PAPARAZZI_HOME)/var/include) @@ -147,7 +156,7 @@ $(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(TOOLS)/gen_modules.out $(CONF)/modules/ $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_modules.out $(SETTINGS_MODULES) $< > $($@_TMP) + $(Q)$(TOOLS)/gen_modules.out $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ diff --git a/README.md b/README.md index 8400e0d98a..86ced4a95d 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,6 @@ Debian users can use http://paparazzi.enac.fr/debian - **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-omap** toolchain for the optional Gumstix Overo module available on lisa/L. - **paparazzi-jsbsim** is needed for using JSBSim as flight dynamic model for the simulator. @@ -66,4 +65,4 @@ Uploading of the embedded software Flight ------ -1. From the Paparazzi Center, select the flight session and ... do the same than in simulation ! \ No newline at end of file +1. From the Paparazzi Center, select the flight session and ... do the same than in simulation ! diff --git a/conf/Makefile.omap b/conf/Makefile.omap index f53a726bc0..9cc359d7cd 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -78,17 +78,12 @@ elf: $(OBJDIR)/$(TARGET).elf # Program the device and start it. load upload program: $(OBJDIR)/$(TARGET).elf -# If it is not the SDK version, then kill program.elf -ifneq ($(BOARD_TYPE), sdk) - -echo "killall -9 program.elf" | telnet $(HOST) -endif - # Kill the application -echo "killall -9 $(TARGET).elf" | telnet $(HOST) - # Upload the modules and start the application + # Make the target dir and edit the config -{ \ - echo "mkdir -p $(TARGET_DIR)"; \ + echo "mkdir -p $(TARGET_DIR)"; \ } | telnet $(HOST) # Upload the drivers and new application @@ -100,13 +95,63 @@ endif } | ftp -n $(HOST) # Upload the modules and start the application - -{ \ - echo "cd $(TARGET_DIR)"; \ - echo "insmod cdc-acm.ko"; \ - echo "chmod 777 $(TARGET).elf"; \ - echo "./$(TARGET).elf > /dev/null 2>&1 &"; \ + -{ \ + 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)"; \ + echo "if grep -q \"start_paparazzi *= \" /data/config.ini; then sed -i 's/\(start_paparazzi *= *\).*/\\\1$(ARDRONE2_START_PAPARAZZI)/g' /data/config.ini; else echo \"start_paparazzi = $(ARDRONE2_START_PAPARAZZI)\" >> /data/config.ini; fi"; \ + echo "if grep -q \"wifi_mode *= \" /data/config.ini; then sed -i 's/\(wifi_mode *= *\).*/\\\1$(ARDRONE2_WIFI_MODE)/g' /data/config.ini; else echo \"wifi_mode = $(ARDRONE2_WIFI_MODE)\" >> /data/config.ini; fi"; \ + echo "if grep -q \"ssid_single_player *= \" /data/config.ini; then sed -i 's/\(ssid_single_player *= *\).*/\\\1$(ARDRONE2_SSID)/g' /data/config.ini; else echo \"ssid_single_player = $(ARDRONE2_SSID)\" >> /data/config.ini; fi"; \ + echo "if grep -q \"static_ip_address_base *= \" /data/config.ini; then sed -i 's/\(static_ip_address_base *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_BASE)/g' /data/config.ini; else echo \"static_ip_address_base = $(ARDRONE2_IP_ADDRESS_BASE)\" >> /data/config.ini; fi"; \ + echo "if grep -q \"static_ip_address_probe *= \" /data/config.ini; then sed -i 's/\(static_ip_address_probe *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_PROBE)/g' /data/config.ini; else echo \"static_ip_address_probe = $(ARDRONE2_IP_ADDRESS_PROBE)\" >> /data/config.ini; fi"; \ + } | 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 $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/check_update.sh check_update.sh"; \ + echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/wifi_setup.sh wifi_setup.sh"; \ + echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \ + echo "quit"; \ + } | ftp -n $(HOST) + + # Upload the modules and start the application + -{ \ + echo "mv /data/video/check_update.sh /bin/"; \ + echo "mv /data/video/wifi_setup.sh /bin/"; \ + echo "chmod 777 /bin/check_update.sh" \ + echo "chmod 777 /bin/wifi_setup.sh" \ + 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 + + + # Link: create ELF output file from object files. .SECONDARY : $(OBJDIR)/$(TARGET).elf @@ -129,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/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index 300eb6eb11..830c176d2f 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -1,12 +1,14 @@ - + - + + + @@ -61,6 +63,7 @@ + @@ -194,7 +197,7 @@ - +
@@ -217,8 +220,8 @@
- +
- + diff --git a/conf/airframes/CDW/conf.xml b/conf/airframes/CDW/conf.xml new file mode 100644 index 0000000000..c8ba362e02 --- /dev/null +++ b/conf/airframes/CDW/conf.xml @@ -0,0 +1,22 @@ + + + + diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 00d9368d88..86946f11f3 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -2,6 +2,8 @@ - + @@ -207,7 +209,7 @@ LiPo/LiIo-Zellen: 2-3
- +
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index ae5f73a8eb..834132e720 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -260,7 +260,7 @@
- +
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml index affb520b20..aaf45e3e27 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/NoVa_L.xml @@ -228,7 +228,7 @@
- +
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml new file mode 100644 index 0000000000..3a14a4338e --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml @@ -0,0 +1,18 @@ + + + +
+ + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml new file mode 100644 index 0000000000..383607ba9b --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml @@ -0,0 +1,20 @@ + + + +
+ + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml new file mode 100644 index 0000000000..92af0c8aac --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml @@ -0,0 +1,23 @@ + + + +
+ + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml new file mode 100644 index 0000000000..e3e8ef76bf --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml @@ -0,0 +1,23 @@ + + + +
+ + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml new file mode 100644 index 0000000000..732f7e7065 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml @@ -0,0 +1,23 @@ + + + +
+ + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml new file mode 100644 index 0000000000..d699f1a956 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml @@ -0,0 +1,18 @@ + + + +
+ + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml new file mode 100644 index 0000000000..b5af215403 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml @@ -0,0 +1,18 @@ + + + +
+ + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml new file mode 100644 index 0000000000..d6dd75562f --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml @@ -0,0 +1,18 @@ + + + +
+ + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml new file mode 100644 index 0000000000..8fd05e01ae --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml @@ -0,0 +1,23 @@ + + + +
+ + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml new file mode 100644 index 0000000000..14840c8c1e --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml @@ -0,0 +1,23 @@ + + + +
+ + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml new file mode 100644 index 0000000000..36ef1433a2 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml @@ -0,0 +1,24 @@ + + + +
+ + + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml new file mode 100644 index 0000000000..294856f650 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml @@ -0,0 +1,23 @@ + + + +
+ + + + + + + + + + + + + + + + + +
+ \ No newline at end of file diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml new file mode 100644 index 0000000000..cf2739449c --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -0,0 +1,206 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + +
+ + + +
+ + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + +
+ +
+ + + + + +
+ diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml new file mode 100644 index 0000000000..7a870f3b9c --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -0,0 +1,223 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/conf.xml b/conf/airframes/TUDelft/IMAV2013/conf.xml new file mode 100644 index 0000000000..dff3725763 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/conf.xml @@ -0,0 +1,232 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml new file mode 100644 index 0000000000..1dc80175b2 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + +
+ + +
+ + +
+ +
+ + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ + diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml new file mode 100644 index 0000000000..064c01ae5a --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -0,0 +1,241 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml new file mode 100644 index 0000000000..2ee2d7548e --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -0,0 +1,196 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml new file mode 100644 index 0000000000..4e7dda0a2e --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -0,0 +1,201 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 87f9e38388..ee9e0848ac 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 @@ -253,7 +253,7 @@
- +
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 f9d1c22a70..06f2f97307 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 @@ -253,7 +253,7 @@
- +
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 0b6eb33d18..8025779613 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -5,11 +5,11 @@ - + @@ -19,15 +19,14 @@ + - - @@ -38,7 +37,7 @@ - + @@ -54,11 +53,12 @@ + - - - - + + + +
@@ -70,36 +70,42 @@
- - - + + + + - - - - - - - - - + + + + + + + + + + + +
- - +
- -
- +
- + + + + + +
@@ -131,7 +137,6 @@
-
@@ -194,22 +199,26 @@
+ + + - +
- +
+ - +
@@ -219,5 +228,4 @@
- diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 154b0126fb..73c21af476 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -6,6 +6,7 @@ + diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index d7467c3203..a62ace8aee 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -232,7 +232,7 @@
- +
diff --git a/conf/airframes/esden/calib/ls01-default.xml b/conf/airframes/esden/calib/ls01-default.xml new file mode 100644 index 0000000000..a7171f0158 --- /dev/null +++ b/conf/airframes/esden/calib/ls01-default.xml @@ -0,0 +1,34 @@ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 6d77ad1dcb..609e7295bd 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -207,7 +207,7 @@ B2L -> CW
- +
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index b38357f11f..02f574bd9f 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -189,7 +189,7 @@
- +
diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index 55412a313b..f796f42c68 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -205,7 +205,7 @@
- +
diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 53292099a9..73554f3209 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -166,7 +166,7 @@
- +
diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index ce6a348d5f..b21dddffcb 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -177,7 +177,7 @@
- +
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index f47a8715d1..c4ef62b938 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -200,7 +200,7 @@
- +
diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index 876ee4a2c3..ca80e5a5d2 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -201,7 +201,7 @@
- +
diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index e94f7008ae..406734e757 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -162,7 +162,7 @@
- +
diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index 6b8b5da46d..e87c817b3e 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -164,7 +164,7 @@
- +
diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index f33377fe79..31fd65c39d 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -162,7 +162,7 @@
- +
diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml new file mode 100644 index 0000000000..4a3a194c39 --- /dev/null +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -0,0 +1,233 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ + + +
+ + + +
+ +
+ + + +
+ +
+ +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml index 9416ac28b8..80dd244402 100644 --- a/conf/airframes/examples/MentorEnergy.xml +++ b/conf/airframes/examples/MentorEnergy.xml @@ -21,6 +21,11 @@ + + + + + @@ -251,4 +256,11 @@ +
+ + + + +
+ diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index dca8389581..e93ccf9d2c 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -194,7 +194,7 @@
- +
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 9b36d7a0f2..4bb3e01b2f 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -226,7 +226,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 0fde4a1238..691a2ea508 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -229,7 +229,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 115801cade..b9fe1162dd 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -218,7 +218,7 @@
- +
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 571700196d..a8bdcd204e 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -214,7 +214,7 @@
- +
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 4deef180d9..5e4869cb87 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -179,7 +179,7 @@
- +
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 0fb7f99e9d..a3d883c653 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -38,6 +38,8 @@ + + @@ -200,10 +202,10 @@
- + - +
@@ -215,7 +217,7 @@
- +
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 90932d24e5..9f7210bd3e 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 @@ -38,7 +38,7 @@ - + @@ -204,7 +204,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 4a20c2b893..4746f8f78d 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -193,7 +193,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml new file mode 100644 index 0000000000..bc0ffa17be --- /dev/null +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -0,0 +1,226 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + + +
+ + +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 17208c6594..611868c6c7 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -202,10 +202,10 @@
- + - +
diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 143f5fb701..98df961abd 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -265,7 +265,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml new file mode 100644 index 0000000000..fb02773791 --- /dev/null +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index 771f4082c8..d5fc8439fd 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -232,7 +232,7 @@ 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 98acc16d07..a2c7336e20 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -29,7 +29,7 @@ - + diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index e1662703b4..528753ac31 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -238,10 +238,10 @@
- + - +
diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index 71ca37d27a..d24b588c2e 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -194,7 +194,7 @@
- +
diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 8d66978f1b..5823e29e03 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -189,7 +189,7 @@
- +
diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index 59f3bb2082..d3f23c31fd 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -201,7 +201,7 @@
- +
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 4061167138..10b61e41a0 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -263,7 +263,7 @@ second attempt
- +
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index 564e7dbcc4..406d6de2c0 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -216,7 +216,7 @@
- +
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 06ea8ce8ad..82f499852b 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -265,7 +265,7 @@ second attempt
- +
diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index f5906c89d6..18ff3d98f8 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -193,7 +193,7 @@
- +
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 17b8eaf5b5..4cf6bb4bbe 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -180,7 +180,7 @@
- +
diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index e3dbdfd842..79e8d519ac 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -217,7 +217,7 @@ AP_MODE_NAV
- +
diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 8066ee667d..37b3b6c07a 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -174,7 +174,7 @@
- +
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 93991b0de9..5d1b3e397d 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml @@ -218,7 +218,7 @@
- +
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 7fabac6e96..5343031357 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 @@ -180,7 +180,7 @@
- +
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 ed99bdefa5..2ba487a784 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 @@ -187,7 +187,7 @@
- +
diff --git a/conf/boards/ardrone2_raw.makefile b/conf/boards/ardrone2_raw.makefile index 5ab2d5f7cc..b97997b11b 100644 --- a/conf/boards/ardrone2_raw.makefile +++ b/conf/boards/ardrone2_raw.makefile @@ -15,11 +15,17 @@ $(TARGET).ARCHDIR = $(ARCH) # ----------------------------------------------------------------------- USER=foobar -HOST=192.168.1.1 +HOST?=192.168.1.1 SUB_DIR=raw FTP_DIR=/data/video TARGET_DIR=$(FTP_DIR)/$(SUB_DIR) # ----------------------------------------------------------------------- +ARDRONE2_START_PAPARAZZI ?= 0 +ARDRONE2_WIFI_MODE ?= 0 +ARDRONE2_SSID ?= ardrone2_paparazzi +ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1. +ARDRONE2_IP_ADDRESS_PROBE ?= 1 +# ----------------------------------------------------------------------- # The GPS sensor is connected trough USB so we have to define the device GPS_PORT ?= UART1 @@ -35,8 +41,8 @@ $(TARGET).CFLAGS +=-DARDRONE2_RAW # ----------------------------------------------------------------------- # default LED configuration -RADIO_CONTROL_LED ?= none -BARO_LED ?= none -AHRS_ALIGNER_LED ?= none -GPS_LED ?= none -SYS_TIME_LED ?= none +RADIO_CONTROL_LED ?= 6 +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 5 +GPS_LED ?= 3 +SYS_TIME_LED ?= 0 diff --git a/conf/boards/ardrone2_sdk.makefile b/conf/boards/ardrone2_sdk.makefile index 46ba01c8c9..6ee6311af1 100644 --- a/conf/boards/ardrone2_sdk.makefile +++ b/conf/boards/ardrone2_sdk.makefile @@ -20,6 +20,12 @@ SUB_DIR=sdk FTP_DIR=/data/video TARGET_DIR=$(FTP_DIR)/$(SUB_DIR) # ----------------------------------------------------------------------- +ARDRONE2_START_PAPARAZZI ?= 0 +ARDRONE2_WIFI_MODE ?= 0 +ARDRONE2_SSID ?= ardrone2_paparazzi +ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1. +ARDRONE2_IP_ADDRESS_PROBE ?= 1 +# ----------------------------------------------------------------------- # The GPS sensor is connected trough USB so we have to define the device GPS_PORT ?= UART1 @@ -32,3 +38,10 @@ $(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\" ap.CFLAGS +=-DARDRONE2_SDK # ----------------------------------------------------------------------- + +# default LED configuration +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= none +GPS_LED ?= none +SYS_TIME_LED ?= none diff --git a/conf/boards/lisa_s_0.1.makefile b/conf/boards/lisa_s_0.1.makefile new file mode 100644 index 0000000000..51d99547e3 --- /dev/null +++ b/conf/boards/lisa_s_0.1.makefile @@ -0,0 +1,72 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# lisa_s_0.1.makefile +# +# http://paparazzi.enac.fr/wiki/Lisa/S +# + +BOARD=lisa_s +BOARD_VERSION=0.1 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/lisa-s.ld + +# ----------------------------------------------------------------------- + +# default flash mode is via usb dfu bootloader (luftboot) +# other possibilities: JTAG, SWD, SERIAL +FLASH_MODE ?= SWD + +# +# +# some default values shared between different firmwares +# +# + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= 3 +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= none +SYS_TIME_LED ?= 1 + +# +# default uart configuration +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 +RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= none + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART3 +GPS_BAUD ?= B38400 + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm + +# Thish should be disabled as we don't have adc inputs on Lisa/S +ifndef ADC_IR1 +ADC_IR1 = 1 +ADC_IR1_CHAN = 0 +endif +ifndef ADC_IR2 +ADC_IR2 = 2 +ADC_IR2_CHAN = 1 +endif +ifndef ADC_IR3 +ADC_IR_TOP = 3 +ADC_IR_TOP_CHAN = 2 +endif +ADC_IR_NB_SAMPLES ?= 16 diff --git a/conf/boards/px4fmu_1.7.makefile b/conf/boards/px4fmu_1.7.makefile new file mode 100644 index 0000000000..798d46d7a5 --- /dev/null +++ b/conf/boards/px4fmu_1.7.makefile @@ -0,0 +1,55 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# px4fmu_1.7.makefile +# +# + +BOARD=px4fmu +BOARD_VERSION=1.7 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +ARCH_L=f4 +ARCH_DIR=stm32 +SRC_ARCH=arch/$(ARCH_DIR) +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/apogee.ld + +HARD_FLOAT=yes + +# default flash mode is via usb dfu bootloader +# possibilities: DFU, SWD +FLASH_MODE ?= SWD + + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= none +SYS_TIME_LED ?= 1 + +# +# default UART configuration (modem, gps, spektrum) +# + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART6 +GPS_BAUD ?= B38400 + +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 + + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm 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.xml.example b/conf/conf_example.xml similarity index 93% rename from conf/conf.xml.example rename to conf/conf_example.xml index dbe5408feb..e8ed823458 100644 --- a/conf/conf.xml.example +++ b/conf/conf_example.xml @@ -44,6 +44,16 @@ 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" /> + +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU +endif + +IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\" +IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c +IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c + +include $(CFG_SHARED)/spi_master.makefile + +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0 +IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1 + +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 -DLISA_S +# Slave select configuration +# SLAVE0 is on PA4 (NSS) (MPU600 CS) +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0 + +# SLAVE1 is on PC4, which is the baro CS +IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1 + +ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_2_SRCS) + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/firmwares/subsystems/shared/imu_nps.makefile b/conf/firmwares/subsystems/shared/imu_nps.makefile index e26d035cd4..a84003b6d6 100644 --- a/conf/firmwares/subsystems/shared/imu_nps.makefile +++ b/conf/firmwares/subsystems/shared/imu_nps.makefile @@ -20,5 +20,5 @@ #
# -nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" +nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" -DUSE_IMU nps.srcs += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c diff --git a/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile new file mode 100644 index 0000000000..68a7bc0099 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile @@ -0,0 +1,70 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# MPU6000 via SPI and HMC5883 via I2C on the PX4FMU v1.7 board +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_PX4FMU_CFLAGS = -DUSE_IMU +endif + +include $(CFG_SHARED)/spi_master.makefile + +IMU_PX4FMU_CFLAGS += -DIMU_TYPE_H=\"imu/imu_px4fmu.h\" +IMU_PX4FMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_PX4FMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu.c +IMU_PX4FMU_SRCS += peripherals/mpu60x0.c +IMU_PX4FMU_SRCS += peripherals/mpu60x0_spi.c +IMU_PX4FMU_CFLAGS += -DUSE_SPI1 +IMU_PX4FMU_CFLAGS += -DUSE_SPI_SLAVE0 -DUSE_SPI_SLAVE1 -DUSE_SPI_SLAVE2 + +# Magnetometer +IMU_PX4FMU_SRCS += peripherals/hmc58xx.c +IMU_PX4FMU_CFLAGS += -DUSE_I2C2 + + +# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets +# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example + +ap.CFLAGS += $(IMU_PX4FMU_CFLAGS) +ap.srcs += $(IMU_PX4FMU_SRCS) + + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/firmwares/subsystems/shared/radio_control_datalink.makefile b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile index bb342a5c3e..d24c413b5a 100644 --- a/conf/firmwares/subsystems/shared/radio_control_datalink.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile @@ -9,11 +9,17 @@ ifeq ($(BOARD),classix) endif endif +RADIO_CONTROL_DATALINK_LED ?= none +RADIO_CONTROL_LED ?= none ifeq ($(NORADIO), False) -ifdef (RADIO_CONTROL_DATALINK_LED) - ap.CFLAGS += -D(RADIO_CONTROL_DATALINK_LED=$((RADIO_CONTROL_DATALINK_LED) -endif + ifneq ($(RADIO_CONTROL_DATALINK_LED),none) + ap.CFLAGS += -DRADIO_CONTROL_DATALINK_LED=$(RADIO_CONTROL_DATALINK_LED) + endif + ifneq ($(RADIO_CONTROL_LED),none) + ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) + fbw.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) + endif $(TARGET).CFLAGS += -DRADIO_CONTROL $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/rc_datalink.h\" $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_DATALINK diff --git a/conf/firmwares/subsystems/shared/radio_control_ppm.makefile b/conf/firmwares/subsystems/shared/radio_control_ppm.makefile index f8e49d96d5..19801b6e74 100644 --- a/conf/firmwares/subsystems/shared/radio_control_ppm.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_ppm.makefile @@ -3,6 +3,7 @@ # NORADIO = False +RADIO_CONTROL_LED ?= none ifeq ($(BOARD),classix) ifeq ($(TARGET),ap) diff --git a/conf/firmwares/subsystems/shared/radio_control_sbus.makefile b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile index 2e8e9ecbe1..67970f298a 100644 --- a/conf/firmwares/subsystems/shared/radio_control_sbus.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile @@ -2,11 +2,14 @@ # Makefile for shared radio_control SBUS subsystem # -$(TARGET).CFLAGS += -DRADIO_CONTROL +RADIO_CONTROL_LED ?= none + ifneq ($(RADIO_CONTROL_LED),none) ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) endif +$(TARGET).CFLAGS += -DRADIO_CONTROL + # convert SBUS_PORT to upper and lower case strings: SBUS_PORT_UPPER=$(shell echo $(SBUS_PORT) | tr a-z A-Z) SBUS_PORT_LOWER=$(shell echo $(SBUS_PORT) | tr A-Z a-z) diff --git a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile index 05d9d555c4..6e92d28d91 100644 --- a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile @@ -1,6 +1,9 @@ # # Makefile for shared radio_control spektrum susbsytem # + +RADIO_CONTROL_LED ?= none + ifndef RADIO_CONTROL_SPEKTRUM_MODEL RADIO_CONTROL_SPEKTRUM_MODEL=\"subsystems/radio_control/spektrum_dx7se.h\" endif diff --git a/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile new file mode 100644 index 0000000000..218a7c194c --- /dev/null +++ b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile @@ -0,0 +1,17 @@ +# +# Makefile for shared radio_control superbitrf subsystem +# + +RADIO_CONTROL_LED ?= none + +ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf_rc.h\" +ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 + +ifneq ($(RADIO_CONTROL_LED),none) +ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) +endif + +ap.srcs += peripherals/cyrf6936.c \ + $(SRC_SUBSYSTEMS)/datalink/superbitrf.c\ + $(SRC_SUBSYSTEMS)/radio_control.c \ + $(SRC_SUBSYSTEMS)/radio_control/superbitrf_rc.c 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/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml index e9798d0de1..de897364e2 100644 --- a/conf/flight_plans/rotorcraft_basic.xml +++ b/conf/flight_plans/rotorcraft_basic.xml @@ -71,8 +71,12 @@ + + + + diff --git a/conf/flight_plans/rotorcraft_basic_superbitrf.xml b/conf/flight_plans/rotorcraft_basic_superbitrf.xml new file mode 100644 index 0000000000..635610b6ae --- /dev/null +++ b/conf/flight_plans/rotorcraft_basic_superbitrf.xml @@ -0,0 +1,113 @@ + + + +
+#include "autopilot.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index dcf3d90931..f08147adaa 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -1,6 +1,6 @@ - +
#include "subsystems/datalink/datalink.h"
diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml index 1427adf72c..d98264c6e6 100644 --- a/conf/flight_plans/versatile_airspeed.xml +++ b/conf/flight_plans/versatile_airspeed.xml @@ -1,6 +1,6 @@ - +
#include "firmwares/fixedwing/guidance/energy_ctrl.h" #include "subsystems/datalink/datalink.h" diff --git a/conf/maps.xml.example b/conf/maps_example.xml similarity index 100% rename from conf/maps.xml.example rename to conf/maps_example.xml diff --git a/conf/messages.xml b/conf/messages.xml index b84ec323c2..92cca695ce 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -48,6 +48,7 @@ + @@ -603,8 +604,21 @@ - - + + + + + + + + + + + + + + + @@ -613,8 +627,22 @@ - - + + + + + + + + + + + + + + + + @@ -1838,7 +1866,13 @@ - + + + + + + + @@ -2218,6 +2252,139 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/flight_time.xml b/conf/modules/flight_time.xml new file mode 100644 index 0000000000..46ab86b367 --- /dev/null +++ b/conf/modules/flight_time.xml @@ -0,0 +1,18 @@ + + + + + + Flight time calculator + Allows to check how much time is left before the end of the competition. + + +
+ +
+ + + + + + \ No newline at end of file diff --git a/conf/modules/light.xml b/conf/modules/light.xml index 33cfd74154..5761db11f1 100644 --- a/conf/modules/light.xml +++ b/conf/modules/light.xml @@ -15,7 +15,7 @@
- + + + + 0.00085 + 18.0 + 2 + 30 + 30 + + + + 0.0 0.0776 + 0.1 0.0744 + 0.2 0.0712 + 0.3 0.0655 + 0.4 0.0588 + 0.5 0.0518 + 0.6 0.0419 + 0.7 0.0318 + 0.8 0.0172 + 1.0 -0.0058 + 1.4 -0.0549 + +
+ + + + 0.0 0.0902 + 0.1 0.0893 + 0.2 0.0880 + 0.3 0.0860 + 0.4 0.0810 + 0.5 0.0742 + 0.6 0.0681 + 0.7 0.0572 + 0.8 0.0467 + 1.0 0.0167 + 1.4 -0.0803 + +
+ + diff --git a/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml b/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml new file mode 100644 index 0000000000..86d43a827e --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml @@ -0,0 +1,8 @@ + + + + + + + 2207.27 + diff --git a/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml b/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml new file mode 100644 index 0000000000..b3dafa86fc --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml @@ -0,0 +1,546 @@ + + + + + + Author Name + Creation Date + Version + Models a Malolo + + + + 10.57 + 9.17 + 1.15 + 1.69 + 3.28 + 1.06 + 0 + + 37.4 + 0 + 0 + + + 20 + 0 + 5 + + + 0 + 0 + 0 + + + + + 1 + 1 + 2 + 0 + 0 + 0 + 12 + + 36.4 + 0 + 4 + + + 1 + + 0 + 0 + 0 + + + + + + + + 40.1 + -9.9 + -10.1 + + 0.8 + 0.5 + 0.02 + 120 + 20 + 0.0 + LEFT + 0 + + + + 40.1 + 9.9 + -10.1 + + 0.8 + 0.5 + 0.02 + 120 + 20 + 0.0 + RIGHT + 0 + + + + 68.9 + 0 + -4.3 + + 0.8 + 0.5 + 0.02 + 24 + 20 + 360.0 + NONE + 0 + + + + 10 + 0 + -8.3 + + 0.8 + 0.5 + 0.02 + 24 + 20 + 360.0 + NONE + 0 + + + + + + + + 36 + 0 + 0 + + + 0.0 + 0 + 0 + + 0 + + + 1 + 0 + 0 + + + 0.0 + 0.0 + 0.0 + + 1.0 + + + + + 36.36 + 0 + -1.89375 + + 1.5 + 1.5 + + + + + + + + fcs/elevator-cmd-norm + fcs/pitch-trim-cmd-norm + + -1 + 1 + + + + + fcs/pitch-trim-sum + + -0.35 + 0.3 + + fcs/elevator-pos-rad + + + + fcs/elevator-pos-rad + + -0.3 + 0.3 + + + -1 + 1 + + fcs/elevator-pos-norm + + + + fcs/aileron-cmd-norm + fcs/roll-trim-cmd-norm + + -1 + 1 + + + + + fcs/roll-trim-sum + + -0.35 + 0.35 + + fcs/left-aileron-pos-rad + + + + -fcs/roll-trim-sum + + -0.35 + 0.35 + + fcs/right-aileron-pos-rad + + + + fcs/left-aileron-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/left-aileron-pos-norm + + + + fcs/right-aileron-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/right-aileron-pos-norm + + + + fcs/rudder-cmd-norm + fcs/yaw-trim-cmd-norm + + -1 + 1 + + + + + fcs/rudder-command-sum + + -0.35 + 0.35 + + fcs/rudder-pos-rad + + + + fcs/rudder-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/rudder-pos-norm + + + + + + + + Drag_at_zero_lift + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-rad + + -1.5700 1.5000 + -0.2600 0.0560 + 0.0000 0.0280 + 0.2600 0.0560 + 1.5700 1.5000 + +
+ + + + Induced_drag + + aero/qbar-psf + metrics/Sw-sqft + aero/cl-squared + 0.0400 + + + + Drag_due_to_sideslip + + aero/qbar-psf + metrics/Sw-sqft + + aero/beta-rad + + -1.5700 1.2300 + -0.2600 0.0500 + 0.0000 0.0000 + 0.2600 0.0500 + 1.5700 1.2300 + +
+ + + + Drag_due_to_Elevator_Deflection + + aero/qbar-psf + metrics/Sw-sqft + fcs/elevator-pos-norm + 0.0300 + + + + + + + Side_force_due_to_beta + + aero/qbar-psf + metrics/Sw-sqft + aero/beta-rad + -1.0000 + + + + + + + Lift_due_to_alpha + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-rad + + -0.2000 -0.7500 + 0.0000 0.2500 + 0.2300 1.4000 + 0.6000 0.7100 + +
+ + + + Lift_due_to_Elevator_Deflection + + aero/qbar-psf + metrics/Sw-sqft + fcs/elevator-pos-rad + 0.2000 + + + + + + + Roll_moment_due_to_beta + + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/beta-rad + -0.1000 + + + + Roll_moment_due_to_roll_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/bi2vel + velocities/p-aero-rad_sec + -0.4000 + + + + Roll_moment_due_to_yaw_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/bi2vel + velocities/r-aero-rad_sec + 0.1500 + + + + Roll_moment_due_to_aileron + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/left-aileron-pos-rad + + velocities/mach + + 0.0000 0.1300 + 2.0000 0.0570 + +
+ + + + Roll_moment_due_to_rudder + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/rudder-pos-rad + 0.0100 + + + + + + + Pitch_moment_due_to_alpha + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + aero/alpha-rad + -0.5000 + + + + Pitch_moment_due_to_elevator + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + fcs/elevator-pos-rad + + velocities/mach + + 0.0000 -0.5000 + 2.0000 -0.2750 + +
+ + + + Pitch_moment_due_to_pitch_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + aero/ci2vel + velocities/q-aero-rad_sec + -12.0000 + + + + Pitch_moment_due_to_alpha_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + aero/ci2vel + aero/alphadot-rad_sec + -7.0000 + + + + + + + Yaw_moment_due_to_beta + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/beta-rad + 0.1200 + + + + Yaw_moment_due_to_yaw_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/bi2vel + velocities/r-aero-rad_sec + -0.1500 + + + + Yaw_moment_due_to_rudder + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/rudder-pos-rad + -0.0500 + + + + Adverse_yaw + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/left-aileron-pos-rad + -0.0300 + + + + Yaw_moment_due_to_tail_incidence + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + 0.0007 + + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 6f83d9b340..2a79fd2c04 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -18,6 +18,7 @@ + diff --git a/data/maps/Makefile b/data/maps/Makefile index f20aff033b..e13be3e9e8 100644 --- a/data/maps/Makefile +++ b/data/maps/Makefile @@ -21,7 +21,7 @@ $(DATADIR): $(DATADIR)/maps.google.com: $(DATADIR) FORCE @echo "-----------------------------------------------" @echo "DOWNLOAD: google maps version code"; - $(Q)wget -q -t 2 -T 20 -O $(@) http://maps.google.com/ || \ + $(Q)wget -q -t 1 -T 10 -O $(@) http://maps.google.com/ || \ (rm -f $(@) && \ echo "Could not download google maps version code" && \ echo "-----------------------------------------------" && \ diff --git a/select_conf.py b/select_conf.py new file mode 100755 index 0000000000..08e91c004a --- /dev/null +++ b/select_conf.py @@ -0,0 +1,262 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import pygtk +import gtk +pygtk.require('2.0') + + +import os +import shutil +import datetime +from fnmatch import fnmatch +import subprocess + + + + +class ConfChooser: + + # General Functions + + def update_combo(self, combo, list): + combo.set_sensitive(False) + combo.get_model().clear() + current_index = 0 + for (i, text) in enumerate(list): + combo.append_text(text) + if os.path.join(self.conf_dir, text) == os.path.realpath(self.conf_xml): + current_index = i + combo.set_active(current_index) + combo.set_sensitive(True) + + def update_label(self): + desc = "Current conf: " + if not os.path.lexists(self.conf_xml): + desc += "does not exist" + else: + if os.path.islink(self.conf_xml): + if os.path.exists(self.conf_xml): + desc += "symlink to " + else: + desc += "broken symlink to " + real_conf_path = os.path.realpath(self.conf_xml) + desc += os.path.relpath(real_conf_path, self.conf_dir) + self.explain.set_text(desc) + + # CallBack Functions + + + def find_conf_files(self): + + conf_files = [] + + pattern = "conf[._-]*xml*" + backup_pattern = "conf[._-]*xml.20[0-9][0-9]-[01][0-9]-[0-3][0-9]_*" + excludes = ["%gconf.xml"] + + for path, subdirs, files in os.walk(self.conf_dir): + for name in files: + if self.exclude_backups and fnmatch(name, backup_pattern): + continue + if fnmatch(name, pattern): + filepath = os.path.join(path, name) + entry = os.path.relpath(filepath, self.conf_dir) + if not os.path.islink(filepath) and entry not in excludes: + conf_files.append(entry) + + conf_files.sort() + self.update_combo(self.conf_file_combo, conf_files) + + + def about(self): + gui_dialogs.about(paparazzi.home_dir) + + def set_backups(self, widget): + self.exclude_backups = not widget.get_active() + self.find_conf_files() + + def launch(self, widget): + os.system("./paparazzi &"); + gtk.main_quit() + + def backupconf(self, use_personal=False): + timestr = datetime.datetime.now().strftime("%Y-%m-%d_%H:%M") + if os.path.islink(self.conf_xml): + if self.verbose: + print("Symlink does not need backup"); + else: + newname = "conf.xml." + timestr + backup_file = os.path.join(self.conf_dir, newname) + shutil.copyfile(self.conf_xml, backup_file) + print("Made a backup: " + newname) + + if use_personal: + backup_name = self.conf_personal_name + "." + timestr + conf_personal_backup = os.path.join(self.conf_dir, backup_name) + if os.path.exists(self.conf_personal): + print("Backup conf.xml.personal to " + backup_name) + shutil.copyfile(self.conf_personal, conf_personal_backup) + + def delete(self, widget): + filename = os.path.join(self.conf_dir, self.conf_file_combo.get_active_text()) + # TODO: dialog: are you certain? + os.remove(filename) + self.update_label() + self.find_conf_files() + + + def accept(self, widget): + selected = self.conf_file_combo.get_active_text() + if selected == "conf.xml": + print("conf.xml is not a symlink, maybe you want to copy it to your personal file first?") + else: + self.backupconf() + link_source = self.conf_file_combo.get_active_text() + os.remove(self.conf_xml) + os.symlink(selected, self.conf_xml) + self.update_label() + self.find_conf_files() + + def personal(self, widget): + if os.path.exists(self.conf_personal): + print("Your personal conf file already exists!") + else: + self.backupconf(True) + template_file = os.path.join(self.conf_dir, self.conf_file_combo.get_active_text()) + shutil.copyfile(template_file, self.conf_personal) + os.remove(self.conf_xml) + os.symlink(self.conf_personal_name, self.conf_xml) + self.update_label() + self.find_conf_files() + + # Constructor Functions + + def destroy(self, widget, data=None): + gtk.main_quit() + + def __init__(self): + self.window = gtk.Window(gtk.WINDOW_TOPLEVEL) + self.window.set_title("Paparazzi Configuration Chooser") + + self.my_vbox = gtk.VBox() + + # if PAPARAZZI_HOME not set, then assume the tree containing this + # file is a reasonable substitute + self.paparazzi_home = os.getenv("PAPARAZZI_HOME", os.path.dirname(os.path.abspath(__file__))) + self.conf_dir = os.path.join(self.paparazzi_home, "conf") + self.conf_xml = os.path.join(self.conf_dir, "conf.xml") + self.conf_personal_name = "conf_personal.xml" + self.conf_personal = os.path.join(self.conf_dir, self.conf_personal_name) + + self.exclude_backups = True + self.verbose = False + + # MenuBar + mb = gtk.MenuBar() + + # File + filemenu = gtk.Menu() + + # File Title + filem = gtk.MenuItem("File") + filem.set_submenu(filemenu) + + exitm = gtk.MenuItem("Exit") + exitm.connect("activate", gtk.main_quit) + filemenu.append(exitm) + + mb.append(filem) + + # Help + helpmenu = gtk.Menu() + + # Help Title + helpm = gtk.MenuItem("Help") + helpm.set_submenu(helpmenu) + + aboutm = gtk.MenuItem("About") + aboutm.connect("activate", self.about) + helpmenu.append(aboutm) + + mb.append(helpm) + + self.my_vbox.pack_start(mb,False) + + # Combo Bar + + self.conf_label = gtk.Label("Conf:") + + self.conf_file_combo = gtk.combo_box_new_text() + self.find_conf_files() +# self.firmwares_combo.connect("changed", self.parse_list_of_airframes) + self.conf_file_combo.set_size_request(600,30) + + self.confbar = gtk.HBox() + self.confbar.pack_start(self.conf_label) + self.confbar.pack_start(self.conf_file_combo) + self.my_vbox.pack_start(self.confbar, False) + + ### show backups button + self.btnBackups = gtk.CheckButton("show backups") + self.btnBackups.connect("toggled", self.set_backups) + self.my_vbox.pack_start(self.btnBackups, False) + + ##### Explain current config + + self.explain = gtk.Label(""); + self.update_label() + + self.exbar = gtk.HBox() + self.exbar.pack_start(self.explain) + + self.my_vbox.pack_start(self.exbar, False) + + ##### Buttons + self.btnAccept = gtk.Button("Set Selected Conf As Active") + self.btnAccept.connect("clicked", self.accept) + self.btnAccept.set_tooltip_text("Set Conf as Active") + + self.btnPersonal = gtk.Button("Create Personal Conf Based on Selected") + self.btnPersonal.connect("clicked", self.personal) + self.btnPersonal.set_tooltip_text("Create Personal Conf Based on Selected and Activate") + + self.btnDelete = gtk.Button("Delete Selected") + self.btnDelete.connect("clicked", self.delete) + self.btnDelete.set_tooltip_text("Permanently Delete") + + self.btnLaunch = gtk.Button("Launch Paparazzi") + self.btnLaunch.connect("clicked", self.launch) + self.btnLaunch.set_tooltip_text("Launch Paparazzi with current conf.xml") + + self.btnExit = gtk.Button("Exit") + self.btnExit.connect("clicked", self.destroy) + self.btnExit.set_tooltip_text("Close application") + + + self.toolbar = gtk.HBox() + self.toolbar.pack_start(self.btnAccept) + self.toolbar.pack_start(self.btnPersonal) + self.toolbar.pack_start(self.btnDelete) + self.toolbar.pack_start(self.btnLaunch) + self.toolbar.pack_start(self.btnExit) + + self.my_vbox.pack_start(self.toolbar, False) + + ##### Bottom + + self.window.add(self.my_vbox) + self.window.show_all() + self.window.set_position(gtk.WIN_POS_CENTER_ALWAYS) + self.window.connect("destroy", self.destroy) + + def main(self): + gtk.main() + +if __name__ == "__main__": + import sys + if (len(sys.argv) > 1): + airframe_file = sys.argv[1] + gui = ConfChooser() + gui.main() diff --git a/sw/airborne/arch/omap/led_hw.h b/sw/airborne/arch/omap/led_hw.h index 884d9581b3..6aecc2884d 100644 --- a/sw/airborne/arch/omap/led_hw.h +++ b/sw/airborne/arch/omap/led_hw.h @@ -28,10 +28,14 @@ #ifndef LED_HW_H_ #define LED_HW_H_ -#define LED_INIT(i) { } -#define LED_ON(i) { } -#define LED_OFF(i) { } -#define LED_TOGGLE(i) { } +#include + +extern uint32_t led_hw_values; + +#define LED_INIT(i) { led_hw_values &= ~(1< + * + * 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 arch/omap/mcu_periph/gpio_arch.h + * + * GPIO helper functions for linux/omap. + * @todo implement gpio_set|clear + */ + +#ifndef GPIO_ARCH_H +#define GPIO_ARCH_H + +#include "std.h" + +/** + * Set a gpio output to high level. + */ +extern void gpio_set(uint32_t port, uint16_t pin); + +/** + * Clear a gpio output to low level. + */ +extern void gpio_clear(uint32_t port, uint16_t pin); + + +/** + * Read a gpio value. + */ +uint16_t gpio_get(uint32_t gpioport, uint16_t gpios); + +#endif /* GPIO_ARCH_H */ diff --git a/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c b/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c index 2d4c78cd04..277e1a7893 100644 --- a/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c @@ -29,6 +29,11 @@ #include #include +#ifdef SYS_TIME_LED +#include "led.h" +#endif + + void sys_time_arch_init( void ) { sys_time.cpu_ticks_per_sec = 1e6; @@ -58,6 +63,10 @@ void sys_tick_handler( int signum ) { if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) { sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec; sys_time.nb_sec++; +#ifdef SYS_TIME_LED + LED_TOGGLE(SYS_TIME_LED); +#endif + } for (unsigned int i=0; i + * + * 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. + */ + +/* Linker script for Lisa-S (STM32F103REY6, 512K flash, 64K RAM). */ + +/* Define memory regions. */ +MEMORY +{ + ram (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + /* Leaving 2k of space at the end of rom for stored settings */ + rom (rx) : ORIGIN = 0x08000000, LENGTH = 512K +} + +/* Include the common ld script. */ +INCLUDE libopencm3_stm32f1.ld + diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c index 6a4ba6bf7e..70e309b299 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c @@ -23,6 +23,8 @@ * STM32 PWM servos handling. */ +//VALID TIMERS ARE TIM1,2,3,4,5,8,9,12 + #include "subsystems/actuators/actuators_pwm_arch.h" #include "subsystems/actuators/actuators_pwm.h" @@ -30,7 +32,6 @@ #include #include -int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; #if defined(STM32F1) //#define PCLK 72000000 @@ -41,6 +42,27 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; #endif #define ONE_MHZ_CLK 1000000 + +#ifdef STM32F1 +/** + * HCLK = 72MHz, Timer clock also 72MHz since + * TIM1_CLK = APB2 = 72MHz + * TIM2_CLK = 2 * APB1 = 2 * 32MHz + */ +#define TIMER_APB1_CLK AHB_CLK +#define TIMER_APB2_CLK AHB_CLK +#endif + +#ifdef STM32F4 +/* Since APB prescaler != 1 : + * Timer clock frequency (before prescaling) is twice the frequency + * of the APB domain to which the timer is connected. + */ +#define TIMER_APB1_CLK (rcc_ppre1_frequency * 2) +#define TIMER_APB2_CLK (rcc_ppre2_frequency * 2) +#endif + + /** Default servo update rate in Hz */ #ifndef SERVO_HZ #define SERVO_HZ 40 @@ -62,19 +84,37 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; #ifndef TIM5_SERVO_HZ #define TIM5_SERVO_HZ SERVO_HZ #endif +#ifndef TIM9_SERVO_HZ +#define TIM9_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM12_SERVO_HZ +#define TIM12_SERVO_HZ SERVO_HZ +#endif + + +/** @todo: these should go into libopencm3 */ +#define TIM9 TIM9_BASE +#define TIM12 TIM12_BASE + +int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; + /** Set PWM channel configuration */ static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, - enum tim_oc_id oc_id) { + enum tim_oc_id oc_id) { timer_disable_oc_output(timer_peripheral, oc_id); - timer_disable_oc_clear(timer_peripheral, oc_id); + //There is no such register in TIM9 and 12. + if (timer_peripheral != TIM9 && timer_peripheral != TIM12) + timer_disable_oc_clear(timer_peripheral, oc_id); timer_enable_oc_preload(timer_peripheral, oc_id); timer_set_oc_slow_mode(timer_peripheral, oc_id); timer_set_oc_mode(timer_peripheral, oc_id, TIM_OCM_PWM1); timer_set_oc_polarity_high(timer_peripheral, oc_id); timer_enable_oc_output(timer_peripheral, oc_id); + // Used for TIM1 and TIM8, the function does nothing if other timer is specified. + timer_enable_break_main_output(timer_peripheral); } /** Set GPIO configuration @@ -103,9 +143,20 @@ static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t chan * - Alignement edge. * - Direction up. */ - timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); + if ((timer == TIM9) || (timer == TIM12)) + //There are no EDGE and DIR settings in TIM9 and TIM12 + timer_set_mode(timer, TIM_CR1_CKD_CK_INT, 0, 0); + else + timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - timer_set_prescaler(timer, (PCLK / ONE_MHZ_CLK) - 1); // 1uS + + // TIM1, 8 and 9 use APB2 clock, all others APB1 + if (timer != TIM1 && timer != TIM8 && timer != TIM9) { + timer_set_prescaler(timer, (TIMER_APB1_CLK / ONE_MHZ_CLK) - 1); // 1uS + } else { + // TIM9, 1 and 8 use APB2 clock + timer_set_prescaler(timer, (TIMER_APB2_CLK / ONE_MHZ_CLK) - 1); + } timer_disable_preload(timer); @@ -148,6 +199,7 @@ static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t chan /* Counter enable. */ timer_enable_counter(timer); + } /** PWM arch init called by generic pwm driver @@ -158,6 +210,7 @@ void actuators_pwm_arch_init(void) { * Configure timer peripheral clocks *-----------------------------------*/ #if PWM_USE_TIM1 + // APB2 runs on double the frequency of APB1. rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM1EN); #endif #if PWM_USE_TIM2 @@ -172,6 +225,17 @@ void actuators_pwm_arch_init(void) { #if PWM_USE_TIM5 rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM5EN); #endif +#if PWM_USE_TIM8 + // APB2 runs on double the frequency of APB1. + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM8EN); +#endif +#if PWM_USE_TIM9 + // APB2 runs on double the frequency of APB1. + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM9EN); +#endif +#if PWM_USE_TIM12 + rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM12EN); +#endif /*---------------- * Configure GPIO @@ -211,6 +275,12 @@ void actuators_pwm_arch_init(void) { #ifdef PWM_SERVO_9 set_servo_gpio(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, PWM_SERVO_9_RCC_IOP); #endif +#ifdef PWM_SERVO_10 + set_servo_gpio(PWM_SERVO_10_GPIO, PWM_SERVO_10_PIN, PWM_SERVO_10_AF, PWM_SERVO_10_RCC_IOP); +#endif +#ifdef PWM_SERVO_11 + set_servo_gpio(PWM_SERVO_11_GPIO, PWM_SERVO_11_PIN, PWM_SERVO_11_AF, PWM_SERVO_11_RCC_IOP); +#endif #if PWM_USE_TIM1 @@ -233,6 +303,18 @@ void actuators_pwm_arch_init(void) { set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK); #endif +#if PWM_USE_TIM8 + set_servo_timer(TIM8, TIM8_SERVO_HZ, PWM_TIM8_CHAN_MASK); +#endif + +#if PWM_USE_TIM9 + set_servo_timer(TIM9, TIM9_SERVO_HZ, PWM_TIM9_CHAN_MASK); +#endif + +#if PWM_USE_TIM12 + set_servo_timer(TIM12, TIM12_SERVO_HZ, PWM_TIM12_CHAN_MASK); +#endif + } /** Set pulse widths from actuator values, assumed to be in us @@ -268,5 +350,11 @@ void actuators_pwm_commit(void) { #ifdef PWM_SERVO_9 timer_set_oc_value(PWM_SERVO_9_TIMER, PWM_SERVO_9_OC, actuators_pwm_values[PWM_SERVO_9]); #endif +#ifdef PWM_SERVO_10 + timer_set_oc_value(PWM_SERVO_10_TIMER, PWM_SERVO_10_OC, actuators_pwm_values[PWM_SERVO_10]); +#endif +#ifdef PWM_SERVO_11 + timer_set_oc_value(PWM_SERVO_11_TIMER, PWM_SERVO_11_OC, actuators_pwm_values[PWM_SERVO_11]); +#endif } diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 353c5a5050..b7ea3f9cae 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -30,7 +30,10 @@ #include "subsystems/actuators.h" #include "actuators_ardrone2_raw.h" -#include "gpio_ardrone.h" +#include "mcu_periph/gpio.h" +#include "led_hw.h" +#include "mcu_periph/sys_time.h" +#include "navdata.h" // for full_write #include /* Standard input/output definitions */ #include /* String function definitions */ @@ -50,24 +53,49 @@ * 190 2.5 * 130 3.0 */ -int mot_fd; /**< File descriptor for the port */ +int actuator_ardrone2_raw_fd; /**< File descriptor for the port */ + +#define ARDRONE_GPIO_PORT 0x32524 + +#define ARDRONE_GPIO_PIN_MOTOR1 171 +#define ARDRONE_GPIO_PIN_MOTOR2 172 +#define ARDRONE_GPIO_PIN_MOTOR3 173 +#define ARDRONE_GPIO_PIN_MOTOR4 174 + +#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175 +#define ARDRONE_GPIO_PIN_IRQ_INPUT 176 + +uint32_t led_hw_values; + +static inline void actuators_ardrone_reset_flipflop(void) +{ + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + int32_t stop = sys_time.nb_sec + 2; + while (sys_time.nb_sec < stop); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); +} + + void actuators_ardrone_init(void) { + led_hw_values = 0; + //open mot port - mot_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY); - if (mot_fd == -1) + actuator_ardrone2_raw_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY); + if (actuator_ardrone2_raw_fd == -1) { perror("open_port: Unable to open /dev/ttyO0 - "); return; } - fcntl(mot_fd, F_SETFL, 0); //read calls are non blocking - fcntl(mot_fd, F_GETFL, 0); + fcntl(actuator_ardrone2_raw_fd, F_SETFL, 0); //read calls are non blocking + fcntl(actuator_ardrone2_raw_fd, F_GETFL, 0); //set port options struct termios options; //Get the current options for the port - tcgetattr(mot_fd, &options); + tcgetattr(actuator_ardrone2_raw_fd, &options); //Set the baud rates to 115200 cfsetispeed(&options, B115200); cfsetospeed(&options, B115200); @@ -78,37 +106,41 @@ void actuators_ardrone_init(void) options.c_oflag &= ~OPOST; //clear output options (raw output) //Set the new options for the port - tcsetattr(mot_fd, TCSANOW, &options); + tcsetattr(actuator_ardrone2_raw_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set(106,-1); - gpio_set(107,0); - gpio_set(107,1); + gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT); + actuators_ardrone_reset_flipflop(); - //all select lines inactive - gpio_set(68,1); - gpio_set(69,1); - gpio_set(70,1); - gpio_set(71,1); + + //all select lines active + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); //configure motors uint8_t reply[256]; for(int m=0;m<4;m++) { - gpio_set(68+m,-1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); actuators_ardrone_cmd(0xe0,reply,2); if(reply[0]!=0xe0 || reply[1]!=0x00) { printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]); } actuators_ardrone_cmd(m+1,reply,1); - gpio_set(68+m,1); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); } //all select lines active - gpio_set(68,-1); - gpio_set(69,-1); - gpio_set(70,-1); - gpio_set(71,-1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); //start multicast actuators_ardrone_cmd(0xa0,reply,1); @@ -117,23 +149,89 @@ void actuators_ardrone_init(void) actuators_ardrone_cmd(0xa0,reply,1); actuators_ardrone_cmd(0xa0,reply,1); - //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_set(106,-1); - gpio_set(107,0); - gpio_set(107,1); + //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0 + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - //all leds green -// actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN); + // Left Red, Right Green + actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); } int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) { - write(mot_fd, &cmd, 1); - return read(mot_fd, reply, replylen); + if (full_write(actuator_ardrone2_raw_fd, &cmd, 1) < 0) + { + perror("actuators_ardrone_cmd: write failed"); + return -1; + } + return full_read(actuator_ardrone2_raw_fd, reply, replylen); +} + +#include "autopilot.h" + +void actuators_ardrone_motor_status(void); +void actuators_ardrone_motor_status(void) +{ + static bool_t last_motor_on = FALSE; + + // Reset Flipflop sequence + static bool_t reset_flipflop_counter = 0; + if (reset_flipflop_counter > 0) + { + reset_flipflop_counter--; + + if (reset_flipflop_counter == 10) + { + // Reset flipflop + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + } + else if (reset_flipflop_counter == 1) + { + // Listen to IRQ again + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + } + return; + } + + // If a motor IRQ line is set + if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) + { + if (autopilot_motors_on) + { + if (last_motor_on) + { + // Tell paparazzi that one motor has stalled + autopilot_set_motors_on(FALSE); + } + else + { + // Toggle Flipflop reset so motors can be re-enabled + reset_flipflop_counter = 20; + } + + } + } + last_motor_on = autopilot_motors_on; + +} + +#define BIT_NUMBER(VAL,BIT) (((VAL)>>BIT)&0x03) + +void actuators_ardrone_led_run(void); +void actuators_ardrone_led_run(void) +{ + static uint32_t previous_led_hw_values = 0x00; + if (previous_led_hw_values != led_hw_values) + { + previous_led_hw_values = led_hw_values; + actuators_ardrone_set_leds(BIT_NUMBER(led_hw_values,0), BIT_NUMBER(led_hw_values,2), BIT_NUMBER(led_hw_values,4), BIT_NUMBER(led_hw_values,6) ); + } } void actuators_ardrone_commit(void) { actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]); + RunOnceEvery(100,actuators_ardrone_motor_status()); } /** @@ -148,22 +246,39 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6); cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7); cmd[4] = ((pwm3&0x1ff)<<1); - write(mot_fd, cmd, 5); + full_write(actuator_ardrone2_raw_fd, cmd, 5); + RunOnceEvery(20,actuators_ardrone_led_run()); } /** * Write LED command - * cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format) + * cmd = 011rrrr0 000gggg0 (this is ardrone1 format, we need ardrone2 format) + * + * + * led0 = RearLeft + * led1 = RearRight + * led2 = FrontRight + * led3 = FrontLeft */ + void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) { uint8_t cmd[2]; - cmd[0]=0x60 | ((led0&3)<<3) | ((led1&3)<<1) | ((led2&3)>>1); - cmd[1]=((led2&3)<<7) | ((led3&3)<<5); - write(mot_fd, cmd, 2); + + led0 &= 0x03; + led1 &= 0x03; + led2 &= 0x03; + led3 &= 0x03; + + printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3); + + cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1); + cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0); + + full_write(actuator_ardrone2_raw_fd, cmd, 2); } void actuators_ardrone_close(void) { - close(mot_fd); + close(actuator_ardrone2_raw_fd); } diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h index 2a66ba5dde..c564dd0ecc 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h @@ -53,6 +53,7 @@ uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB]; extern void actuators_ardrone_commit(void); extern void actuators_ardrone_init(void); + int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen); void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3); void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3); diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c index b2c11e4d0b..eba448ee13 100644 --- a/sw/airborne/boards/ardrone/at_com.c +++ b/sw/airborne/boards/ardrone/at_com.c @@ -133,10 +133,14 @@ void init_at_config(void) { } //Recieve a navdata packet -void at_com_recieve_navdata(unsigned char* buffer) { - int l; - recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0, - (struct sockaddr *) &from, (socklen_t *) &l); +int at_com_recieve_navdata(unsigned char* buffer) { + int l = sizeof(from); + int n; + // FIXME(ben): not clear why recvfrom() and not recv() is used. + n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0, + (struct sockaddr *) &from, (socklen_t *) &l); + + return n; } //Send an AT command diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h index f2b04185b2..79084315ef 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -202,7 +202,7 @@ typedef struct _navdata_gps_t { //External functions extern void init_at_com(void); -extern void at_com_recieve_navdata(unsigned char* buffer); +extern int at_com_recieve_navdata(unsigned char* buffer); extern void at_com_send_config(char* key, char* value); extern void at_com_send_ftrim(void); extern void at_com_send_ref(int bits); diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index ce0a1591a6..9b05dad93a 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -69,7 +69,7 @@ static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw) return (baro_calibration.b5 + 8) >> 4; } -void baro_periodic(void) +void baro_periodic(void) { } @@ -77,8 +77,8 @@ 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); + 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) { diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c index f47f0157bd..6ffc6a8f5a 100644 --- a/sw/airborne/boards/ardrone/electrical_raw.c +++ b/sw/airborne/boards/ardrone/electrical_raw.c @@ -68,10 +68,12 @@ static struct { int fd; void electrical_init(void) { + // First we try to kill the program.elf if it is running (done here because initializes first) + int ret = system("killall -9 program.elf"); + (void) ret; + // Initialize 12c device for power fd = open( "/dev/i2c-1", O_RDWR ); - - if ( ioctl( fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) { fprintf( stderr, "Failed to set slave address: %m\n" ); } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 3a08b1c267..363ca87686 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -22,18 +22,112 @@ * ardrone GPIO driver */ -#include -#include -#include "gpio_ardrone.h" +#ifdef ARDRONE2_RAW -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -//val=-1 -> set gpio as input (output hi-Z) -int gpio_set(int nr,int val) +#include /* File control definitions */ +#include /* Error number definitions */ +#include + +#include "mcu_periph/gpio.h" + +#define GPIO_MAGIC 'p' +#define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, struct gpio_direction) +#define GPIO_READ _IOWR(GPIO_MAGIC, 1, struct gpio_data) +#define GPIO_WRITE _IOW(GPIO_MAGIC, 2, struct gpio_data) +int gpiofp = 0; + +struct gpio_data { + int pin; + int value; +}; + +enum gpio_mode { + GPIO_INPUT = 0, //!< Pin configured for input + GPIO_OUTPUT_LOW, //!< Pin configured for output with low level + GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level +}; + +struct gpio_direction { + int pin; + enum gpio_mode mode; +}; + + +void gpio_set(uint32_t port, uint16_t pin) { - char cmdline[200]; - if(val<0) sprintf(cmdline,"/usr/sbin/gpio %d -d i",nr); - else if(val>0) sprintf(cmdline,"/usr/sbin/gpio %d -d ho 1",nr); - else sprintf(cmdline,"/usr/sbin/gpio %d -d ho 0",nr); - return system(cmdline); + if (port != 0x32524) return; // protect ardrone board from unauthorized use + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = pin; + data.value = 1; + ioctl(gpiofp, GPIO_WRITE, &data); } + + +void gpio_clear(uint32_t port, uint16_t pin) +{ + if (port != 0x32524) return; // protect ardrone board from unauthorized use + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = pin; + data.value = 0; + ioctl(gpiofp, GPIO_WRITE, &data); +} + + +void gpio_setup_input(uint32_t port, uint16_t pin) +{ + if (port != 0x32524) return; // protect ardrone board from unauthorized use + struct gpio_direction dir; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + dir.pin = pin; + dir.mode = GPIO_INPUT; + ioctl(gpiofp, GPIO_DIRECTION, &dir); +} + + +void gpio_setup_output(uint32_t port, uint16_t pin) +{ + /* + if (port != 0x32524) return; // protect ardrone board from unauthorized use + struct gpio_direction dir; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + dir.pin = pin; + dir.mode = GPIO_OUTPUT_LOW; + ioctl(gpiofp, GPIO_DIRECTION, &dir); + */ +} + + + +uint16_t gpio_get(uint32_t port, uint16_t pin) +{ + if (port != 0x32524) return 0; // protect ardrone board from unauthorized use + struct gpio_data data; + // Open the device if not open + if (gpiofp == 0) + gpiofp = open("/dev/gpio",O_RDWR); + + // Read the GPIO value + data.pin = pin; + ioctl(gpiofp, GPIO_READ, &data); + return data.value; +} + +#endif diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.h b/sw/airborne/boards/ardrone/gpio_ardrone.h deleted file mode 100644 index 55563f2e09..0000000000 --- a/sw/airborne/boards/ardrone/gpio_ardrone.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301 USA. - */ - -/** - * @file boards/ardrone/gpio_ardrone.h - * ardrone GPIO driver - */ - -#ifndef GPIO_ARDRONE_H -#define GPIO_ARDRONE_H - -//val=0 -> set gpio output lo -//val=1 -> set gpio output hi -//val=-1 -> set gpio as input (output hi-Z) -int gpio_set(int nr,int val); - -#endif /* GPIO_ARDRONE_H */ diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 683426faba..e4f43a136a 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -34,21 +34,81 @@ #include #include #include +#include +#include + +#include "std.h" #include "navdata.h" +#include "subsystems/ins.h" -int nav_fd; +#define NAVDATA_PACKET_SIZE 60 +#define NAVDATA_START_BYTE 0x3a -int navdata_init() +static inline bool_t acquire_baro_calibration(void); +static void navdata_cropbuffer(int cropsize); + +navdata_port nav_port; +static int nav_fd = 0; +static int16_t previousUltrasoundHeight; +measures_t navdata; + +#include "subsystems/sonar.h" +uint16_t sonar_meas = 0; + + +// FIXME(ben): there must be a better home for these +ssize_t full_write(int fd, const uint8_t *buf, size_t count) { - port = malloc(sizeof(navdata_port)); + size_t written = 0; - nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); - if (nav_fd == -1) + while(written < count) { - perror("navdata_init: Unable to open /dev/ttyO1 - "); - return 1; - } else { - port->isOpen = 1; + ssize_t n = write(fd, buf + written, count - written); + if (n < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + continue; + return n; + } + written += n; + } + return written; +} + +ssize_t full_read(int fd, uint8_t *buf, size_t count) +{ + // Apologies for illiteracy, but we can't overload |read|. + size_t readed = 0; + + while(readed < count) + { + ssize_t n = read(fd, buf + readed, count - readed); + if (n < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + continue; + return n; + } + readed += n; + } + return readed; +} + +static void navdata_write(const uint8_t *buf, size_t count) +{ + if (full_write(nav_fd, buf, count) < 0) + perror("navdata_write: Write failed"); +} + +bool_t navdata_init() +{ + if (nav_fd <= 0) { + nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (nav_fd == -1) { + perror("navdata_init: Unable to open /dev/ttyO1 - "); + return FALSE; + } } fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking @@ -70,49 +130,52 @@ int navdata_init() // stop acquisition uint8_t cmd=0x02; - write(nav_fd, &cmd, 1); + navdata_write(&cmd, 1); - baro_calibrated = 0; - acquire_baro_calibration(); + // read some potential dirt (retry alot of times) + char tmp[100]; + for(int i = 0; i < 100; i++) { + uint16_t dirt = read(nav_fd, tmp, sizeof tmp); + (void) dirt; + + cmd=0x02; + navdata_write(&cmd, 1); + usleep(200); + } + + baro_calibrated = FALSE; + if(!acquire_baro_calibration()) + return FALSE; // start acquisition - cmd=0x01; - write(nav_fd, &cmd, 1); + cmd = 0x01; + navdata_write(&cmd, 1); - navdata = malloc(sizeof(measures_t)); - navdata_imu_available = 0; - navdata_baro_available = 0; - - port->bytesRead = 0; - port->totalBytesRead = 0; - port->packetsRead = 0; - port->isInitialized = 1; + navdata_imu_available = FALSE; + navdata_baro_available = FALSE; previousUltrasoundHeight = 0; + nav_port.checksum_errors = 0; + nav_port.bytesRead = 0; + nav_port.totalBytesRead = 0; + nav_port.packetsRead = 0; + nav_port.isInitialized = TRUE; - return 0; + return TRUE; } -void acquire_baro_calibration() +static inline bool_t acquire_baro_calibration(void) { - read(nav_fd, NULL, 100); // read some potential dirt - // start baro calibration acquisition uint8_t cmd=0x17; // send cmd 23 - write(nav_fd, &cmd, 1); + navdata_write(&cmd, 1); uint8_t calibBuffer[22]; - int calib_bytes_read, calib_bytes_left, readcount; - calib_bytes_read = 0; - calib_bytes_left = 22; - readcount = 0; - while(calib_bytes_read != 22) { - readcount = read(nav_fd, calibBuffer+(22-calib_bytes_left), calib_bytes_left); - if (readcount > 0) { - calib_bytes_read += readcount; - calib_bytes_left -= readcount; - } + if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) + { + perror("acquire_baro_calibration: read failed"); + return FALSE; } baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; @@ -127,8 +190,6 @@ void acquire_baro_calibration() baro_calibration.mc = calibBuffer[18] << 8 | calibBuffer[19]; baro_calibration.md = calibBuffer[20] << 8 | calibBuffer[21]; - printf("Calibration bytes read: %d\n", calib_bytes_read); - printf("Calibration AC1: %d\n", baro_calibration.ac1); printf("Calibration AC2: %d\n", baro_calibration.ac2); printf("Calibration AC3: %d\n", baro_calibration.ac3); @@ -143,37 +204,23 @@ void acquire_baro_calibration() printf("Calibration MC: %d\n", baro_calibration.mc); printf("Calibration MD: %d\n", baro_calibration.md); - baro_calibrated = 1; -} - -void navdata_close() -{ - port->isOpen = 0; - close(nav_fd); + baro_calibrated = TRUE; + return TRUE; } void navdata_read() { - int newbytes = 0; - - if (port->isInitialized != 1) - navdata_init(); - - if (port->isOpen != 1) - return; - - newbytes = read(nav_fd, port->buffer+port->bytesRead, NAVDATA_BUFFER_SIZE-port->bytesRead); + int newbytes = read(nav_fd, nav_port.buffer+nav_port.bytesRead, NAVDATA_BUFFER_SIZE-nav_port.bytesRead); // because non-blocking read returns -1 when no bytes available if (newbytes > 0) { - port->bytesRead += newbytes; - port->totalBytesRead += newbytes; + nav_port.bytesRead += newbytes; + nav_port.totalBytesRead += newbytes; } } -void baro_update_logic(void); -void baro_update_logic(void) +static void baro_update_logic(void) { static int32_t lastpressval = 0; static uint16_t lasttempval = 0; @@ -184,157 +231,145 @@ void baro_update_logic(void) if (temp_or_press_was_updated_last == 0) // Last update was press so we are now waiting for temp { // temp was updated - temp_or_press_was_updated_last = 1; - + temp_or_press_was_updated_last = TRUE; + // This means that press must remain constant if (lastpressval != 0) { // If pressure was updated: this is a sync error - if (lastpressval != navdata->pressure) + if (lastpressval != navdata.pressure) { // wait for temp again - temp_or_press_was_updated_last = 0; + temp_or_press_was_updated_last = FALSE; sync_errors++; - navdata_baro_available = 1; + navdata_baro_available = TRUE; } } } else { // press was updated - temp_or_press_was_updated_last = 0; + temp_or_press_was_updated_last = FALSE; // This means that temp must remain constant if (lasttempval != 0) { // If temp was updated: this is a sync error - if (lasttempval != navdata->temperature_pressure) + if (lasttempval != navdata.temperature_pressure) { // wait for press again - temp_or_press_was_updated_last = 1; + temp_or_press_was_updated_last = TRUE; sync_errors++; } } - navdata_baro_available = 1; + navdata_baro_available = TRUE; } - lastpressval = navdata->pressure; - lasttempval = navdata->temperature_pressure; - - // debug - // navdata->temperature_pressure = sync_errors; + lastpressval = navdata.pressure; + lasttempval = navdata.temperature_pressure; } void navdata_update() { + static bool_t last_checksum_wrong = FALSE; + // Check if initialized + if (!nav_port.isInitialized) { + navdata_init(); + return; + } + + // Start reading navdata_read(); // while there is something interesting to do... - while (port->bytesRead >= 60) + while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE) { - if (port->buffer[0] == NAVDATA_START_BYTE) + if (nav_port.buffer[0] == NAVDATA_START_BYTE) { - // if checksum is OK - if ( 1 ) // we dont know how to calculate the checksum -// if ( navdata_checksum() == 0 ) - { - memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE); - + assert(sizeof navdata == NAVDATA_PACKET_SIZE); + memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE); + + // Calculating the checksum + uint16_t checksum = 0; + for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) { + checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8); + } + + // When checksum is incorrect + if(navdata.chksum != checksum) { + printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum); + nav_port.checksum_errors++; + } + + // When we already dropped a packet or checksum is correct + if(last_checksum_wrong || navdata.chksum == checksum) { // Invert byte order so that TELEMETRY works better uint8_t tmp; - uint8_t* p = (uint8_t*) &(navdata->pressure); + uint8_t* p = (uint8_t*) &(navdata.pressure); tmp = p[0]; p[0] = p[1]; p[1] = tmp; - p = (uint8_t*) &(navdata->temperature_pressure); + p = (uint8_t*) &(navdata.temperature_pressure); tmp = p[0]; p[0] = p[1]; p[1] = tmp; baro_update_logic(); - navdata_imu_available = 1; +#ifdef USE_SONAR + if (navdata.ultrasound < 10000) + { + sonar_meas = navdata.ultrasound; + ins_update_sonar(); - port->packetsRead++; -// printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum)); - //navdata_getHeight(); + } +#endif + + + navdata_imu_available = TRUE; + last_checksum_wrong = FALSE; + nav_port.packetsRead++; } - navdata_CropBuffer(60); + + // Crop the buffer + navdata_cropbuffer(NAVDATA_PACKET_SIZE); } else { // find start byte, copy all data from startbyte to buffer origin, update bytesread uint8_t * pint; - pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead); + pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead); if (pint != NULL) { - navdata_CropBuffer(pint - port->buffer); + navdata_cropbuffer(pint - nav_port.buffer); } else { // if the start byte was not found, it means there is junk in the buffer - port->bytesRead = 0; + nav_port.bytesRead = 0; } } } } -void navdata_CropBuffer(int cropsize) -{ - if (port->bytesRead - cropsize < 0) { - // TODO think about why the amount of bytes read minus the cropsize gets below zero - printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port->bytesRead, cropsize, port->buffer); - return; - } - - memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize); - port->bytesRead -= cropsize; -} - -int16_t navdata_getHeight() { - - if (navdata->ultrasound > 10000) { +int16_t navdata_height(void) { + if (navdata.ultrasound > 10000) { return previousUltrasoundHeight; } int16_t ultrasoundHeight = 0; - - ultrasoundHeight = (navdata->ultrasound - 880) / 26.553; - + ultrasoundHeight = (navdata.ultrasound - 880) / 26.553; previousUltrasoundHeight = ultrasoundHeight; - return ultrasoundHeight; } -// The checksum should be calculated here: we don't know the algorithm -uint16_t navdata_checksum() { - navdata_cks = 0; - navdata_cks += navdata->nu_trame; - navdata_cks += navdata->ax; - navdata_cks += navdata->ay; - navdata_cks += navdata->az; - navdata_cks += navdata->vx; - navdata_cks += navdata->vy; - navdata_cks += navdata->vz; - navdata_cks += navdata->temperature_acc; - navdata_cks += navdata->temperature_gyro; - navdata_cks += navdata->ultrasound; - navdata_cks += navdata->us_debut_echo; - navdata_cks += navdata->us_fin_echo; - navdata_cks += navdata->us_association_echo; - navdata_cks += navdata->us_distance_echo; - navdata_cks += navdata->us_curve_time; - navdata_cks += navdata->us_curve_value; - navdata_cks += navdata->us_curve_ref; - navdata_cks += navdata->nb_echo; - navdata_cks += navdata->sum_echo; - navdata_cks += navdata->gradient; - navdata_cks += navdata->flag_echo_ini; - navdata_cks += navdata->pressure; - navdata_cks += navdata->temperature_pressure; - navdata_cks += navdata->mx; - navdata_cks += navdata->my; - navdata_cks += navdata->mz; -// navdata_cks += navdata->chksum; +static void navdata_cropbuffer(int cropsize) +{ + if (nav_port.bytesRead - cropsize < 0) { + // TODO think about why the amount of bytes read minus the cropsize gets below zero + printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, nav_port.buffer); + return; + } - return 0; // we dont know how to calculate the checksum + memmove(nav_port.buffer, nav_port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize); + nav_port.bytesRead -= cropsize; } diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h index 922bb2e46b..f41b52a9c0 100644 --- a/sw/airborne/boards/ardrone/navdata.h +++ b/sw/airborne/boards/ardrone/navdata.h @@ -31,19 +31,7 @@ #define NAVDATA_H_ #include - -#define NAVDATA_PACKET_SIZE 60 -#define NAVDATA_BUFFER_SIZE 80 -#define NAVDATA_START_BYTE 0x3a - -typedef struct { - uint8_t isInitialized; - uint8_t isOpen; - uint16_t bytesRead; - uint32_t totalBytesRead; - uint32_t packetsRead; - uint8_t buffer[NAVDATA_BUFFER_SIZE]; -} navdata_port; +#include typedef struct { @@ -107,25 +95,31 @@ struct bmp180_baro_calibration int32_t b5; }; -measures_t* navdata; +#define NAVDATA_BUFFER_SIZE 80 +typedef struct { + uint8_t isInitialized; + uint16_t bytesRead; + uint32_t totalBytesRead; + uint32_t packetsRead; + uint32_t checksum_errors; + uint8_t buffer[NAVDATA_BUFFER_SIZE]; +} navdata_port; + +extern measures_t navdata; +extern navdata_port nav_port; struct bmp180_baro_calibration baro_calibration; navdata_port* port; uint16_t navdata_cks; uint8_t navdata_imu_available; uint8_t navdata_baro_available; -int16_t previousUltrasoundHeight; uint8_t baro_calibrated; -int navdata_init(void); -void navdata_close(void); - +bool_t navdata_init(void); void navdata_read(void); void navdata_update(void); -void navdata_CropBuffer(int cropsize); +int16_t navdata_height(void); -uint16_t navdata_checksum(void); -int16_t navdata_getHeight(void); - -void acquire_baro_calibration(void); +ssize_t full_write(int fd, const uint8_t *buf, size_t count); +ssize_t full_read(int fd, uint8_t *buf, size_t count); #endif /* NAVDATA_H_ */ diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c index e120cbbea3..8d369be25a 100644 --- a/sw/airborne/boards/krooz/imu_krooz.c +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -180,7 +180,7 @@ void imu_krooz_event( void ) // If the HMC5883 I2C transaction has succeeded: convert the data hmc58xx_event(&imu_krooz.hmc); if (imu_krooz.hmc.data_available) { - VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.z, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.y); + VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z); UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); imu_krooz.hmc.data_available = FALSE; imu_krooz.mag_valid = TRUE; diff --git a/sw/airborne/boards/lisa_s/baro_board.h b/sw/airborne/boards/lisa_s/baro_board.h new file mode 100644 index 0000000000..7581ef4de3 --- /dev/null +++ b/sw/airborne/boards/lisa_s/baro_board.h @@ -0,0 +1,16 @@ + +/* + * board specific functions for the lisa_m board + * + */ + +#ifndef BOARDS_LISA_M_BARO_H +#define BOARDS_LISA_M_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_LISA_M_BARO_H */ diff --git a/sw/airborne/boards/lisa_s_0.1.h b/sw/airborne/boards/lisa_s_0.1.h new file mode 100644 index 0000000000..1182c1954e --- /dev/null +++ b/sw/airborne/boards/lisa_s_0.1.h @@ -0,0 +1,429 @@ +#ifndef CONFIG_LISA_S_0_1_H +#define CONFIG_LISA_S_0_1_H + +#define BOARD_LISA_S + +/* Lisa/M has a 12MHz external clock and 72MHz internal. */ +#define EXT_CLK 12000000 +#define AHB_CLK 72000000 + +/* + * Onboard LEDs + */ + +/* red, on PA8 */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOC +#define LED_1_GPIO_CLK RCC_APB2ENR_IOPCEN +#define LED_1_GPIO_PIN GPIO10 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +/* green, shared with JTAG_TRST */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOC +#define LED_2_GPIO_CLK RCC_APB2ENR_IOPCEN +#define LED_2_GPIO_PIN GPIO11 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + +/* green, shared with ADC12 (ADC_6 on connector ANALOG2) */ +#ifndef USE_LED_3 +#define USE_LED_3 1 +#endif +#define LED_3_GPIO GPIOD +#define LED_3_GPIO_CLK RCC_APB2ENR_IOPDEN +#define LED_3_GPIO_PIN GPIO2 +#define LED_3_GPIO_ON gpio_clear +#define LED_3_GPIO_OFF gpio_set +#define LED_3_AFIO_REMAP ((void)0) + +/* + * not actual LEDS, used as GPIOs + */ + +/* PB1, DRDY on EXT SPI connector*/ +#define LED_BODY_GPIO GPIOB +#define LED_BODY_GPIO_CLK RCC_APB2ENR_IOPBEN +#define LED_BODY_GPIO_PIN GPIO1 +#define LED_BODY_GPIO_ON gpio_set +#define LED_BODY_GPIO_OFF gpio_clear +#define LED_BODY_AFIO_REMAP ((void)0) + +/* PC12, on GPIO connector*/ +#define LED_12_GPIO GPIOC +#define LED_12_GPIO_CLK RCC_APB2ENR_IOPCEN +#define LED_12_GPIO_PIN GPIO12 +#define LED_12_GPIO_ON gpio_clear +#define LED_12_GPIO_OFF gpio_set +#define LED_12_AFIO_REMAP ((void)0) + + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + + +#define DefaultVoltageOfAdc(adc) (0.0045*adc) + +/* Onboard ADCs */ +/* + ADC1 PC3/ADC13 + ADC2 PC0/ADC10 + ADC3 PC1/ADC11 + ADC4 PC5/ADC15 + ADC6 PC2/ADC12 + BATT PC4/ADC14 +*/ +/* We should remove the first three adc channels, as Lisa/S does not provide those. */ +#define BOARD_ADC_CHANNEL_1 10 +#define BOARD_ADC_CHANNEL_2 11 +#define BOARD_ADC_CHANNEL_3 12 +// we can only use ADC1,2,3; the last channel is for bat monitoring +#define BOARD_ADC_CHANNEL_4 2 + +/* 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 + */ +#define ADC_1 0 +#define ADC_2 1 +#define ADC_3 2 + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY 3 +#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 locm3 lib specific +#ifdef USE_AD1 +#define ADC1_GPIO_INIT(gpio) { \ + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, \ + GPIO_CNF_INPUT_ANALOG, \ + GPIO2); \ + } +#endif // USE_AD1 + +#define BOARD_HAS_BARO 1 + +/* SPI slave mapping */ + +/* IMU_MPU_CS */ +#define SPI_SELECT_SLAVE0_PORT GPIOA +#define SPI_SELECT_SLAVE0_PIN GPIO4 + +/* IMU_BARO_CS */ +#define SPI_SELECT_SLAVE1_PORT GPIOC +#define SPI_SELECT_SLAVE1_PIN GPIO4 + +/* RADIO_SS */ +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO12 + +/* + * UART pin configuration + * + * sets on which pins the UARTs are connected + */ +/* DIAG port */ +#define UART1_GPIO_AF 0 +#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX +#define UART1_GPIO_RX GPIO_USART1_RX +#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX +#define UART1_GPIO_TX GPIO_USART1_TX + +/* GPS */ +#define UART3_GPIO_AF 0 +#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_RX +#define UART3_GPIO_RX GPIO_USART3_RX +#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_TX +#define UART3_GPIO_TX GPIO_USART3_TX + +/* LED1 & LED2 */ +/* +#define UART3_GPIO_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP +#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_PR_RX +#define UART3_GPIO_RX GPIO_USART3_PR_RX +#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_PR_TX +#define UART3_GPIO_TX GPIO_USART3_PR_TX +*/ + +/* + * Spektrum + */ +/* The line that is pulled low at power up to initiate the bind process */ +#define SPEKTRUM_BIND_PIN GPIO2 +#define SPEKTRUM_BIND_PIN_PORT GPIOB + +/* Diag Port RX */ +#define SPEKTRUM_UART1_RCC_REG &RCC_APB2ENR +#define SPEKTRUM_UART1_RCC_DEV RCC_APB2ENR_USART1EN +#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX +#define SPEKTRUM_UART1_PIN GPIO_USART1_RX +#define SPEKTRUM_UART1_AF 0 +#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ +#define SPEKTRUM_UART1_ISR usart1_isr +#define SPEKTRUM_UART1_DEV USART1 + +/* AUX Radio RX */ +#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN +#define SPEKTRUM_UART2_BANK GPIO_BANK_USART2_RX +#define SPEKTRUM_UART2_PIN GPIO_USART2_RX +#define SPEKTRUM_UART2_AF 0 +#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ +#define SPEKTRUM_UART2_ISR usart2_isr +#define SPEKTRUM_UART2_DEV USART2 + +/* LED2 */ +#define SPEKTRUM_UART3_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART3_RCC_DEV RCC_APB1ENR_USART3EN +#define SPEKTRUM_UART3_BANK GPIO_BANK_USART3_PR_RX +#define SPEKTRUM_UART3_PIN GPIO_USART3_PR_RX +#define SPEKTRUM_UART3_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP +#define SPEKTRUM_UART3_IRQ NVIC_USART3_IRQ +#define SPEKTRUM_UART3_ISR usart3_isr +#define SPEKTRUM_UART3_DEV USART3 + +/* LED3 */ +#define SPEKTRUM_UART5_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART5_RCC_DEV RCC_APB1ENR_UART5EN +#define SPEKTRUM_UART5_BANK GPIO_BANK_UART5_RX +#define SPEKTRUM_UART5_PIN GPIO_UART5_RX +#define SPEKTRUM_UART5_AF 0 +#define SPEKTRUM_UART5_IRQ NVIC_UART5_IRQ +#define SPEKTRUM_UART5_ISR uart5_isr +#define SPEKTRUM_UART5_DEV UART5 + +/* PPM + */ + +/* + * On Lisa/S there is no really dedicated port available. But we could use a + * bunch of different pins to do PPM Input. + */ + +/* + * Default is PPM config 2, input on GPIO01 (Servo pin 6) + */ + +#ifndef PPM_CONFIG +#define PPM_CONFIG 3 +#endif + + +#if PPM_CONFIG == 1 +/* input on PA01 (UART1_RX) Diag port */ +#define USE_PPM_TIM1 1 +#define PPM_CHANNEL TIM_IC3 +#define PPM_TIMER_INPUT TIM_IC_IN_TI3 +#define PPM_IRQ NVIC_TIM1_UP_IRQ +#define PPM_IRQ2 NVIC_TIM1_CC_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC3IE +#define PPM_CC_IF TIM_SR_CC3IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO10 +#define PPM_GPIO_AF 0 + +#elif PPM_CONFIG == 2 +/* input on PA1 (Servo 6 pin) + On Lisa/S we could also use TIM5 IC2 instead. */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC2 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC2IE +#define PPM_CC_IF TIM_SR_CC2IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO1 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#elif PPM_CONFIG == 3 +/* input on PA3 (Aux RX pin) + On Lisa/S we could also use TIM5 IC4 instead. */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC4 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC4IE +#define PPM_CC_IF TIM_SR_CC4IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO3 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#else +#error "Unknown PPM config" + +#endif // PPM_CONFIG + +/* ADC */ + +// active ADC +#define USE_AD1 1 +#define USE_AD1_1 1 +#define USE_AD1_2 1 +#define USE_AD1_3 1 +#define USE_AD1_4 1 + +/* + * I2C + * + */ +/* Servo1 & Servo2 */ +#define I2C1_GPIO_PORT GPIOB +#define I2C1_GPIO_SCL GPIO6 +#define I2C1_GPIO_SDA GPIO7 + +/* GPS TX & RX */ +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + +/* + * PWM + * + */ +#define PWM_USE_TIM4 1 +#define PWM_USE_TIM5 1 + + +#if USE_SERVOS_1AND2 + #if USE_I2C1 + #error "You cannot USE_SERVOS_1AND2 and USE_I2C1 at the same time" + #else + #define ACTUATORS_PWM_NB 6 + #define USE_PWM1 1 + #define USE_PWM2 1 + #define PWM_USE_TIM4 1 + #endif +#else + #define ACTUATORS_PWM_NB 4 +#endif + +#define USE_PWM3 1 +#define USE_PWM4 1 +#define USE_PWM5 1 +#define USE_PWM6 1 + +// Servo numbering on LisaM silkscreen/docs starts with 1 + +/* PWM_SERVO_x is the index of the servo in the actuators_pwm_values array */ +/* Because PWM1 and 2 can be disabled we put them at the index 4 & 5 so that it + * is easy to disable. + */ +#if USE_PWM1 +#define PWM_SERVO_1 4 +#define PWM_SERVO_1_TIMER TIM4 +#define PWM_SERVO_1_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_1_GPIO GPIOB +#define PWM_SERVO_1_PIN GPIO6 +#define PWM_SERVO_1_AF 0 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 5 +#define PWM_SERVO_2_TIMER TIM4 +#define PWM_SERVO_2_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_2_GPIO GPIOB +#define PWM_SERVO_2_PIN GPIO7 +#define PWM_SERVO_2_AF 0 +#define PWM_SERVO_2_OC TIM_OC2 +#define PWM_SERVO_2_OC_BIT (1<<1) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 0 +#define PWM_SERVO_3_TIMER TIM4 +#define PWM_SERVO_3_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO8 +#define PWM_SERVO_3_AF 0 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 1 +#define PWM_SERVO_4_TIMER TIM4 +#define PWM_SERVO_4_RCC_IOP RCC_APB2ENR_IOPBEN +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO9 +#define PWM_SERVO_4_AF 0 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 2 +#define PWM_SERVO_5_TIMER TIM5 +#define PWM_SERVO_5_RCC_IOP RCC_APB2ENR_IOPAEN +#define PWM_SERVO_5_GPIO GPIOA +#define PWM_SERVO_5_PIN GPIO0 +#define PWM_SERVO_5_AF 0 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + +#if USE_PWM6 +#define PWM_SERVO_6 3 +#define PWM_SERVO_6_TIMER TIM5 +#define PWM_SERVO_6_RCC_IOP RCC_APB2ENR_IOPAEN +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO1 +#define PWM_SERVO_6_AF 0 +#define PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + +/* servos 1-4 or 3-4 on TIM4 depending on USE_SERVOS_1AND2 */ +#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) +/* servos 5-6 on TIM5 */ +#define PWM_TIM5_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT) + +/* SuperbitRF mounted */ +#define SUPERBITRF_SPI_DEV spi2 +#define SUPERBITRF_RST_PORT GPIOC +#define SUPERBITRF_RST_PIN GPIO9 +#define SUPERBITRF_DRDY_PORT GPIOC +#define SUPERBITRF_DRDY_PIN GPIO6 +#define SUPERBITRF_FORCE_DSM2 FALSE + +#endif /* CONFIG_LISA_S_0_1_H */ diff --git a/sw/airborne/boards/px4fmu/baro_board.h b/sw/airborne/boards/px4fmu/baro_board.h new file mode 100644 index 0000000000..4aff1cc1c7 --- /dev/null +++ b/sw/airborne/boards/px4fmu/baro_board.h @@ -0,0 +1,14 @@ + +/* + * board specific fonctions for the PX4FMU board + * + */ + +#ifndef BOARDS_PX4FMU_BARO_H +#define BOARDS_PX4FMU_BARO_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_PX4FMU_BARO_H */ diff --git a/sw/airborne/boards/px4fmu_1.7.h b/sw/airborne/boards/px4fmu_1.7.h new file mode 100644 index 0000000000..deb299a1d6 --- /dev/null +++ b/sw/airborne/boards/px4fmu_1.7.h @@ -0,0 +1,313 @@ +#ifndef CONFIG_PX4FMU_1_7_H +#define CONFIG_PX4FMU_1_7_H + +#define BOARD_PX4FMU + +/* PX4 has a 16MHz external clock and 168MHz internal. */ +#define EXT_CLK 16000000 +#define AHB_CLK 168000000 + + +/* + * Onboard LEDs + */ +/* red/amber , on PB14 */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOB +#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_1_GPIO_PIN GPIO14 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +/* blue, on PB15 */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOB +#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_2_GPIO_PIN GPIO15 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + + + +/* + * UART + */ +#define UART1_GPIO_AF GPIO_AF7 +#define UART1_GPIO_PORT_RX GPIOB +#define UART1_GPIO_RX GPIO7 +#define UART1_GPIO_PORT_TX GPIOB +#define UART1_GPIO_TX GPIO6 + +#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 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 + + +/* + * SPI + */ +/* SPI1 for MPU and accel/gyro if populated */ +#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 + +/* SPI3 on microSD connector */ +#define SPI3_GPIO_AF GPIO_AF5 +#define SPI3_GPIO_PORT_MISO GPIOC +#define SPI3_GPIO_MISO GPIO11 +#define SPI3_GPIO_PORT_MOSI GPIOB +#define SPI3_GPIO_MOSI GPIO5 +#define SPI3_GPIO_PORT_SCK GPIOC +#define SPI3_GPIO_SCK GPIO10 + +/* + * SPI slave pin declaration + */ +/* GYRO_CS on SPI1 */ +#define SPI_SELECT_SLAVE0_PORT GPIOC +#define SPI_SELECT_SLAVE0_PIN GPIO14 + +/* ACCEL_CS on SPI1 */ +#define SPI_SELECT_SLAVE1_PORT GPIOC +#define SPI_SELECT_SLAVE1_PIN GPIO15 + +/* MPU_CS on SPI1 */ +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO0 + +/* SPI3 NSS on microSD connector */ +#define SPI_SELECT_SLAVE3_PORT GPIOA +#define SPI_SELECT_SLAVE3_PIN GPIO4 + + +/* Onboard ADCs */ +#define USE_AD_TIM4 1 + +#define BOARD_ADC_CHANNEL_1 11 +#define BOARD_ADC_CHANNEL_2 12 +#define BOARD_ADC_CHANNEL_3 13 +#define BOARD_ADC_CHANNEL_4 10 + +#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 + */ +#define ADC_1 ADC1_C1 +#ifdef USE_ADC_1 +#ifndef ADC_1_GPIO_CLOCK_PORT +#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_1_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1) +#endif +#define USE_AD1_1 1 +#else +#define ADC_1_GPIO_CLOCK_PORT 0 +#define ADC_1_INIT() {} +#endif + +#define ADC_2 ADC1_C2 +#ifdef USE_ADC_2 +#ifndef ADC_2_GPIO_CLOCK_PORT +#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_2_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO2) +#endif +#define USE_AD1_2 1 +#else +#define ADC_2_GPIO_CLOCK_PORT 0 +#define ADC_2_INIT() {} +#endif + +#define ADC_3 ADC1_C3 +#ifdef USE_ADC_3 +#ifndef ADC_3_GPIO_CLOCK_PORT +#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO3) +#endif +#define USE_AD1_3 1 +#else +#define ADC_3_GPIO_CLOCK_PORT 0 +#define ADC_3_INIT() {} +#endif + +#define ADC_4 ADC1_C4 +#ifndef ADC_4_GPIO_CLOCK_PORT +#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN +#define ADC_4_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0) +#endif +#define USE_AD1_4 1 + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY ADC_4 +#endif +#define DefaultVoltageOfAdc(adc) (0.006185*adc) + +#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT) + +/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ +#ifdef USE_AD1 +#define ADC1_GPIO_INIT(gpio) { \ + ADC_1_INIT(); \ + ADC_2_INIT(); \ + ADC_3_INIT(); \ + ADC_4_INIT(); \ +} +#endif // USE_AD1 + + +/* + * I2C mapping + */ +#define I2C1_GPIO_PORT GPIOB +#define I2C1_GPIO_SCL GPIO8 +#define I2C1_GPIO_SDA GPIO9 + +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + +#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 + +#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 GPIO10 +#define PPM_GPIO_AF GPIO_AF1 + + +/* + * Spektrum + */ +/* The line that is pulled low at power up to initiate the bind process */ +/* GPIO_EXT1 on PX4FMU */ +#define SPEKTRUM_BIND_PIN GPIO4 +#define SPEKTRUM_BIND_PIN_PORT GPIOC + +#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR +#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN +#define SPEKTRUM_UART2_BANK GPIOA +#define SPEKTRUM_UART2_PIN GPIO3 +#define SPEKTRUM_UART2_AF GPIO_AF7 +#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ +#define SPEKTRUM_UART2_ISR usart2_isr +#define SPEKTRUM_UART2_DEV USART2 + + + +/* Activate onboard baro */ +#define BOARD_HAS_BARO 1 + + + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + +/* PWM */ +#define PWM_USE_TIM2 1 + +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 + +// Servo numbering on the PX4 starts with 1 + +// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array +// SRV1 is also UART2_CTS +#if USE_PWM1 +#define PWM_SERVO_1 0 +#define PWM_SERVO_1_TIMER TIM2 +#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_1_GPIO GPIOA +#define PWM_SERVO_1_PIN GPIO0 +#define PWM_SERVO_1_AF GPIO_AF1 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +// SRV2 is also UART2_RTS +#if USE_PWM2 +#define PWM_SERVO_2 1 +#define PWM_SERVO_2_TIMER TIM2 +#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_2_GPIO GPIOA +#define PWM_SERVO_2_PIN GPIO1 +#define PWM_SERVO_2_AF GPIO_AF1 +#define PWM_SERVO_2_OC TIM_OC2 +#define PWM_SERVO_2_OC_BIT (1<<1) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +// SRV3 is also UART2_TX +#if USE_PWM3 +#define PWM_SERVO_3_IDX 2 +#define PWM_SERVO_3_TIMER TIM2 +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_3_GPIO GPIOA +#define PWM_SERVO_3_PIN GPIO2 +#define PWM_SERVO_3_AF GPIO_AF1 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +// SRV4 is also UART2_RX +#if USE_PWM4 +#define PWM_SERVO_4 3 +#define PWM_SERVO_4_TIMER TIM2 +#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_4_GPIO GPIOA +#define PWM_SERVO_4_PIN GPIO3 +#define PWM_SERVO_4_AF GPIO_AF1 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + + +#define PWM_TIM2_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) + + +#endif /* CONFIG_PX4FMU_1_7_H */ diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h index 288385d132..4884eb932d 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 + * 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 SKL + * 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/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 27095c67eb..6ec32bf2da 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -168,9 +168,9 @@ float ac_char_cruise_throttle = 0.0f; float ac_char_cruise_pitch = 0.0f; int ac_char_cruise_count = 0; -static void ac_char_average(float* last, float new, int count) +static void ac_char_average(float* last_v, float new_v, int count) { - *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count); + *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count); } static void ac_char_update(float throttle, float pitch, float climb, float accelerate) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 8c1e8bc7be..3ad0007e58 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -84,6 +84,10 @@ #include "led.h" +#ifdef USE_NPS +#include "nps_autopilot_fixedwing.h" +#endif + /* Default trim commands for roll, pitch and yaw */ #ifndef COMMAND_ROLL_TRIM #define COMMAND_ROLL_TRIM 0 @@ -102,14 +106,14 @@ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY) PRINT_CONFIG_VAR(CONTROL_FREQUENCY) -#ifndef TELEMETRY_FREQUENCY -#define TELEMETRY_FREQUENCY 60 -#endif +/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h + * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file + */ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY) -#ifndef MODULES_FREQUENCY -#define MODULES_FREQUENCY 60 -#endif +/* MODULES_FREQUENCY is defined in generated/modules.h + * according to main_freq parameter set for modules in airframe file + */ PRINT_CONFIG_VAR(MODULES_FREQUENCY) @@ -575,7 +579,7 @@ void sensors_task( void ) { #endif // USE_IMU //FIXME: this is just a kludge -#if USE_AHRS && defined SITL +#if USE_AHRS && defined SITL && !USE_NPS ahrs_propagate(); #endif @@ -720,10 +724,6 @@ static inline void on_gyro_event( void ) { ahrs_propagate(); ahrs_update_accel(); -#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP - new_ins_attitude = 1; -#endif - #else //PERIODIC_FREQUENCY static uint8_t _reduced_propagation_rate = 0; static uint8_t _reduced_correction_rate = 0; @@ -736,6 +736,7 @@ static inline void on_gyro_event( void ) { _reduced_propagation_rate++; if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY)) { + return; } else { @@ -757,13 +758,17 @@ static inline void on_gyro_event( void ) { ImuScaleAccel(imu); ahrs_update_accel(); } - -#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP - new_ins_attitude = 1; -#endif } #endif //PERIODIC_FREQUENCY +#if defined SITL && USE_NPS + if (nps_bypass_ahrs) sim_overwrite_ahrs(); +#endif + +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP + new_ins_attitude = 1; +#endif + } static inline void on_mag_event(void) diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index d4c5c2b5b8..701a644b82 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -57,6 +57,9 @@ uint8_t fbw_mode; #include "inter_mcu.h" +#ifdef USE_NPS +#include "nps_autopilot_fixedwing.h" +#endif /** Trim commands for roll, pitch and yaw. * These are updated from the trim commands in ap_state via inter_mcu diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 41c21a3343..adf1490c9b 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -49,10 +49,25 @@ bool_t kill_throttle; bool_t autopilot_rc; bool_t autopilot_power_switch; -bool_t autopilot_detect_ground; +bool_t autopilot_ground_detected; bool_t autopilot_detect_ground_once; -#define AUTOPILOT_IN_FLIGHT_TIME 40 +#define AUTOPILOT_IN_FLIGHT_TIME 20 + +/** minimum vertical speed for in_flight condition in m/s */ +#ifndef AUTOPILOT_IN_FLIGHT_MIN_SPEED +#define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2 +#endif + +/** minimum vertical acceleration for in_flight condition in m/s^2 */ +#ifndef AUTOPILOT_IN_FLIGHT_MIN_ACCEL +#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0 +#endif + +/** minimum thrust for in_flight condition in pprz_t units */ +#ifndef AUTOPILOT_IN_FLIGHT_MIN_THRUST +#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500 +#endif #ifndef AUTOPILOT_DISABLE_AHRS_KILL #include "subsystems/ahrs.h" @@ -90,7 +105,7 @@ void autopilot_init(void) { autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; autopilot_mode_auto2 = MODE_AUTO2; - autopilot_detect_ground = FALSE; + autopilot_ground_detected = FALSE; autopilot_detect_ground_once = FALSE; autopilot_flight_time = 0; autopilot_rc = TRUE; @@ -106,59 +121,42 @@ void autopilot_init(void) { guidance_v_init(); stabilization_init(); - /* set startup mode, propagats through to guidance h/v */ + /* set startup mode, propagates through to guidance h/v */ autopilot_set_mode(MODE_STARTUP); } -static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) { - if (autopilot_in_flight) { - if (autopilot_in_flight_counter > 0) { - if (stabilization_cmd[COMMAND_THRUST] == 0) { - autopilot_in_flight_counter--; - if (autopilot_in_flight_counter == 0) { - autopilot_in_flight = FALSE; - } - } - else { /* !THROTTLE_STICK_DOWN */ - autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; - } - } - } - else { /* not in flight */ - if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && - motors_on) { - if (stabilization_cmd[COMMAND_THRUST] > 0) { - autopilot_in_flight_counter++; - if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) - autopilot_in_flight = TRUE; - } - else { /* THROTTLE_STICK_DOWN */ - autopilot_in_flight_counter = 0; - } - } - } -} - - void autopilot_periodic(void) { RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); -#if FAILSAFE_GROUND_DETECT -INFO("Using FAILSAFE_GROUND_DETECT") - if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) { - autopilot_set_mode(AP_MODE_KILL); - autopilot_detect_ground = FALSE; - } -#endif - /* set failsafe commands, if in FAILSAFE or KILL mode */ -#if !FAILSAFE_GROUND_DETECT - if (autopilot_mode == AP_MODE_KILL || - autopilot_mode == AP_MODE_FAILSAFE) { -#else - if (autopilot_mode == AP_MODE_KILL) { + + /* If in FAILSAFE mode and either already not in_flight anymore + * or just "detected" ground, go to KILL mode. + */ + if (autopilot_mode == AP_MODE_FAILSAFE) { + if (!autopilot_in_flight) + autopilot_set_mode(AP_MODE_KILL); + +#if FAILSAFE_GROUND_DETECT +INFO("Using FAILSAFE_GROUND_DETECT: KILL") + if (autopilot_ground_detected) + autopilot_set_mode(AP_MODE_KILL); #endif + } + + /* Reset ground detection _after_ running flight plan + */ + if (!autopilot_in_flight || autopilot_ground_detected) { + autopilot_ground_detected = FALSE; + autopilot_detect_ground_once = FALSE; + } + + /* Set fixed "failsafe" commands from airframe file if in KILL mode. + * If in FAILSAFE mode, run normal loops with failsafe attitude and + * downwards velocity setpoints. + */ + if (autopilot_mode == AP_MODE_KILL) { SetCommands(commands_failsafe); } else { @@ -167,10 +165,6 @@ INFO("Using FAILSAFE_GROUND_DETECT") SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } - // when we dont have RC, check in flight by looking at throttle - if (radio_control.status != RC_OK) { - autopilot_check_in_flight_no_rc(autopilot_motors_on); - } } @@ -190,7 +184,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { break; #endif case AP_MODE_KILL: - autopilot_set_motors_on(FALSE); autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); @@ -234,6 +227,8 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { break; #endif case AP_MODE_KILL: + autopilot_set_motors_on(FALSE); + stabilization_cmd[COMMAND_THRUST] = 0; guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RC_DIRECT: @@ -269,29 +264,37 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { } -static inline void autopilot_check_in_flight( bool_t motors_on ) { +void autopilot_check_in_flight(bool_t motors_on) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { - if (THROTTLE_STICK_DOWN()) { + /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */ + if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) && + (abs(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) && + (abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) + { autopilot_in_flight_counter--; if (autopilot_in_flight_counter == 0) { autopilot_in_flight = FALSE; } } - else { /* !THROTTLE_STICK_DOWN */ + else { /* thrust, speed or accel not above min threshold, reset counter */ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; } } } - else { /* not in flight */ + else { /* currently not in flight */ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && - motors_on) { - if (!THROTTLE_STICK_DOWN()) { + motors_on) + { + /* if thrust above min threshold, assume in_flight. + * Don't check for velocity and acceleration above threshold here... + */ + if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) { autopilot_in_flight_counter++; if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) autopilot_in_flight = TRUE; } - else { /* THROTTLE_STICK_DOWN */ + else { /* currently not in_flight and thrust below threshold, reset counter */ autopilot_in_flight_counter = 0; } } @@ -344,8 +347,6 @@ void autopilot_on_rc_frame(void) { autopilot_arming_check_motors_on(); kill_throttle = ! autopilot_motors_on; - autopilot_check_in_flight(autopilot_motors_on); - guidance_v_read_rc(); guidance_h_read_rc(autopilot_in_flight); } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 7e874ef0f3..9203c6e43f 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -67,8 +67,9 @@ extern void autopilot_periodic(void); extern void autopilot_on_rc_frame(void); extern void autopilot_set_mode(uint8_t new_autopilot_mode); extern void autopilot_set_motors_on(bool_t motors_on); +extern void autopilot_check_in_flight(bool_t motors_on); -extern bool_t autopilot_detect_ground; +extern bool_t autopilot_ground_detected; extern bool_t autopilot_detect_ground_once; extern uint16_t autopilot_flight_time; @@ -151,7 +152,7 @@ static inline void DetectGroundEvent(void) { struct NedCoor_f* accel = stateGetAccelNed_f(); if (accel->z < -THRESHOLD_GROUND_DETECT || accel->z > THRESHOLD_GROUND_DETECT) { - autopilot_detect_ground = TRUE; + autopilot_ground_detected = TRUE; autopilot_detect_ground_once = FALSE; } } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 39995aad2f..842f3ccf21 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -139,29 +139,35 @@ void guidance_h_mode_changed(uint8_t new_mode) { stabilization_attitude_reset_care_free_heading(); case GUIDANCE_H_MODE_FORWARD: case GUIDANCE_H_MODE_ATTITUDE: +#if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ - if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || - guidance_h_mode == GUIDANCE_H_MODE_RATE) { + if (guidance_h_mode == GUIDANCE_H_MODE_KILL || + guidance_h_mode == GUIDANCE_H_MODE_RATE || + guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT) +#endif stabilization_attitude_enter(); - } break; case GUIDANCE_H_MODE_HOVER: guidance_h_hover_enter(); +#if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ - if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || - guidance_h_mode == GUIDANCE_H_MODE_RATE) { + if (guidance_h_mode == GUIDANCE_H_MODE_KILL || + guidance_h_mode == GUIDANCE_H_MODE_RATE || + guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT) +#endif stabilization_attitude_enter(); - } break; case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); +#if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ - if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || - guidance_h_mode == GUIDANCE_H_MODE_RATE) { + if (guidance_h_mode == GUIDANCE_H_MODE_KILL || + guidance_h_mode == GUIDANCE_H_MODE_RATE || + guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT) +#endif stabilization_attitude_enter(); - } break; default: @@ -253,12 +259,12 @@ void guidance_h_run(bool_t in_flight) { guidance_h_nav_enter(); if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { - struct Int32Eulers sp_euler_i; - sp_euler_i.phi = nav_roll; - sp_euler_i.theta = nav_pitch; + 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 */ - sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi; - stabilization_attitude_set_from_eulers_i(&sp_euler_i); + sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi; + stabilization_attitude_set_cmd_i(&sp_cmd_i); } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -267,6 +273,7 @@ void guidance_h_run(bool_t in_flight) { /* 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_traj_run(in_flight); } @@ -362,8 +369,8 @@ static void guidance_h_traj_run(bool_t in_flight) { 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-eulers */ - stabilization_attitude_set_from_eulers_i(&guidance_h_command_body); + /* Set attitude setpoint from pseudo-euler commands */ + stabilization_attitude_set_cmd_i(&guidance_h_command_body); } static void guidance_h_hover_enter(void) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index b1db6ebcfa..34b23d212c 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -53,7 +53,7 @@ struct Int64Vect2 gh_pos_ref; #ifndef GUIDANCE_H_REF_MAX_ACCEL #define GUIDANCE_H_REF_MAX_ACCEL 5.66 #endif -#define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC) +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 @@ -61,7 +61,7 @@ struct Int64Vect2 gh_pos_ref; #endif /** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ #define GH_MAX_SPEED_REF_FRAC 7 -#define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC) +static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); /** second order model natural frequency */ #ifndef GUIDANCE_H_REF_OMEGA @@ -72,14 +72,16 @@ struct Int64Vect2 gh_pos_ref; #define GUIDANCE_H_REF_ZETA 0.85 #endif #define GH_ZETA_OMEGA_FRAC 10 -#define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC) #define GH_OMEGA_2_FRAC 7 -#define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC) +static const int32_t gh_zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC); +static const int32_t gh_omega_2= BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC); /** first order time constant */ -#define GH_REF_THAU_F 0.5 -#define GH_REF_INV_THAU_FRAC 16 -#define GH_REF_INV_THAU BFP_OF_REAL((1./GH_REF_THAU_F), GH_REF_INV_THAU_FRAC) +#ifndef GUIDANCE_H_REF_TAU +#define GUIDANCE_H_REF_TAU 0.5 +#endif +#define GH_REF_INV_TAU_FRAC 16 +static const int32_t gh_ref_inv_tau = BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC); static struct Int32Vect2 gh_max_speed_ref; static struct Int32Vect2 gh_max_accel_ref; @@ -88,6 +90,13 @@ static int32_t route_ref; static int32_t s_route_ref; static int32_t c_route_ref; +static void gh_compute_route_ref(struct Int32Vect2* ref_vector); +static void gh_compute_ref_max(struct Int32Vect2* ref_vector); +static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector); +static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector); +static void gh_saturate_ref_accel(void); +static void gh_saturate_ref_speed(void); + void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { struct Int64Vect2 new_pos; new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); @@ -105,7 +114,7 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp) struct Int32Vect2 speed; INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); - VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA); + VECT2_SMUL(speed, speed, -2 * gh_zeta_omega); INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC); // compute pos error in pos_sp resolution struct Int32Vect2 pos_err; @@ -115,65 +124,19 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC)); // compute the "pos part" of accel struct Int32Vect2 pos; - VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2)); + VECT2_SMUL(pos, pos_err, -gh_omega_2); INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC); // sum accel VECT2_SUM(gh_accel_ref, speed, pos); - /* Compute route reference before saturation */ - float f_route_ref = atan2f(-pos_err.y, -pos_err.x); - route_ref = ANGLE_BFP_OF_REAL(f_route_ref); - /* Compute North and East route components */ - PPRZ_ITRIG_SIN(s_route_ref, route_ref); - PPRZ_ITRIG_COS(c_route_ref, route_ref); - c_route_ref = abs(c_route_ref); - s_route_ref = abs(s_route_ref); - /* Compute maximum acceleration*/ - gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); - gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); - /* Compute maximum speed*/ - gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); - gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); - /* restore gh_speed_ref range (Q14.17) */ - INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + /* Compute max ref accel/speed along route before saturation */ + gh_compute_ref_max(&pos_err); - /* Saturate accelerations */ - if (gh_accel_ref.x <= -gh_max_accel_ref.x) { - gh_accel_ref.x = -gh_max_accel_ref.x; - } - else if (gh_accel_ref.x >= gh_max_accel_ref.x) { - gh_accel_ref.x = gh_max_accel_ref.x; - } - if (gh_accel_ref.y <= -gh_max_accel_ref.y) { - gh_accel_ref.y = -gh_max_accel_ref.y; - } - else if (gh_accel_ref.y >= gh_max_accel_ref.y) { - gh_accel_ref.y = gh_max_accel_ref.y; - } - - /* Saturate speed and adjust acceleration accordingly */ - if (gh_speed_ref.x <= -gh_max_speed_ref.x) { - gh_speed_ref.x = -gh_max_speed_ref.x; - if (gh_accel_ref.x < 0) - gh_accel_ref.x = 0; - } - else if (gh_speed_ref.x >= gh_max_speed_ref.x) { - gh_speed_ref.x = gh_max_speed_ref.x; - if (gh_accel_ref.x > 0) - gh_accel_ref.x = 0; - } - if (gh_speed_ref.y <= -gh_max_speed_ref.y) { - gh_speed_ref.y = -gh_max_speed_ref.y; - if (gh_accel_ref.y < 0) - gh_accel_ref.y = 0; - } - else if (gh_speed_ref.y >= gh_max_speed_ref.y) { - gh_speed_ref.y = gh_max_speed_ref.y; - if (gh_accel_ref.y > 0) - gh_accel_ref.y = 0; - } + gh_saturate_ref_accel(); + gh_saturate_ref_speed(); } + void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { /* WARNING: SPEED SATURATION UNTESTED */ VECT2_ADD(gh_pos_ref, gh_speed_ref); @@ -186,26 +149,80 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { // convert to accel resolution INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); // compute accel from speed_sp - VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU); - INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC); + VECT2_SMUL(gh_accel_ref, speed_err, -gh_ref_inv_tau); + INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_TAU_FRAC); - /* Compute route reference before saturation */ - float f_route_ref = atan2f(-speed_sp.y, -speed_sp.x); + /* Compute max ref accel/speed along route before saturation */ + gh_compute_ref_max_speed(&speed_sp); + gh_compute_ref_max_accel(&speed_err); + + gh_saturate_ref_accel(); + gh_saturate_ref_speed(); +} + +static void gh_compute_route_ref(struct Int32Vect2* ref_vector) { + float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x); route_ref = ANGLE_BFP_OF_REAL(f_route_ref); /* Compute North and East route components */ PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref); c_route_ref = abs(c_route_ref); s_route_ref = abs(s_route_ref); - /* Compute maximum acceleration*/ - gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); - gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); - /* Compute maximum speed*/ - gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); - gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); - /* restore gh_speed_ref range (Q14.17) */ - INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); +} +static void gh_compute_ref_max(struct Int32Vect2* ref_vector) { + /* Compute route reference before saturation */ + if (ref_vector->x == 0 && ref_vector->y == 0) { + gh_max_accel_ref.x = 0; + gh_max_accel_ref.y = 0; + gh_max_speed_ref.x = 0; + gh_max_speed_ref.y = 0; + } + else { + gh_compute_route_ref(ref_vector); + /* Compute maximum acceleration*/ + gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC); + gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC); + /* Compute maximum speed*/ + gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC); + /* restore gh_speed_ref range (Q14.17) */ + INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + } +} + +static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) { + /* Compute route reference before saturation */ + if (ref_vector->x == 0 && ref_vector->y == 0) { + gh_max_accel_ref.x = 0; + gh_max_accel_ref.y = 0; + } + else { + gh_compute_route_ref(ref_vector); + /* Compute maximum acceleration*/ + gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC); + gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC); + } +} + +static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) { + /* Compute route reference before saturation */ + if (ref_vector->x == 0 && ref_vector->y == 0) { + gh_max_speed_ref.x = 0; + gh_max_speed_ref.y = 0; + } + else { + gh_compute_route_ref(ref_vector); + /* Compute maximum speed*/ + gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC); + /* restore gh_speed_ref range (Q14.17) */ + INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + } +} + +/** saturate reference accelerations */ +static void gh_saturate_ref_accel(void) { /* Saturate accelerations */ if (gh_accel_ref.x <= -gh_max_accel_ref.x) { gh_accel_ref.x = -gh_max_accel_ref.x; @@ -219,8 +236,10 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { else if (gh_accel_ref.y >= gh_max_accel_ref.y) { gh_accel_ref.y = gh_max_accel_ref.y; } +} - /* Saturate speed and adjust acceleration accordingly */ +/** Saturate ref speed and adjust acceleration accordingly */ +static void gh_saturate_ref_speed(void) { if (gh_speed_ref.x <= -gh_max_speed_ref.x) { gh_speed_ref.x = -gh_max_speed_ref.x; if (gh_accel_ref.x < 0) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 432f655f1b..23f625b442 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -94,6 +94,8 @@ int32_t guidance_v_ki; int32_t guidance_v_z_sum_err; +static int32_t guidance_v_thrust_coeff; + #define GuidanceVSetRef(_pos, _speed, _accel) { \ gv_set_ref(_pos, _speed, _accel); \ @@ -102,8 +104,8 @@ int32_t guidance_v_z_sum_err; guidance_v_zdd_ref = _accel; \ } - -void run_hover_loop(bool_t in_flight); +static int32_t get_vertical_thrust_coeff(void); +static void run_hover_loop(bool_t in_flight); void guidance_v_init(void) { @@ -174,8 +176,10 @@ void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co + guidance_v_thrust_coeff = get_vertical_thrust_coeff(); if (in_flight) { - gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref); + int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; + gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref); } else { /* reset estimate while not in_flight */ @@ -217,6 +221,7 @@ void guidance_v_run(bool_t in_flight) { if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER) guidance_v_z_sp = fms.input.v_sp.height; #endif + guidance_v_zd_sp = 0; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); #if NO_RC_THRUST_LIMIT @@ -231,17 +236,21 @@ void guidance_v_run(bool_t in_flight) { { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; + guidance_v_zd_sp = 0; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_CLIMB) { + guidance_v_z_sp = stateGetPositionNed_i()->z; guidance_v_zd_sp = -nav_climb; gv_update_ref_from_zd_sp(guidance_v_zd_sp); - nav_flight_altitude = -guidance_v_z_sp; run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_MANUAL) { - guidance_v_z_sp = -nav_flight_altitude; // For display only + guidance_v_z_sp = stateGetPositionNed_i()->z; + guidance_v_zd_sp = stateGetSpeedNed_i()->z; + GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0); + guidance_v_z_sum_err = 0; guidance_v_delta_t = nav_throttle; } #if NO_RC_THRUST_LIMIT @@ -260,12 +269,35 @@ void guidance_v_run(bool_t in_flight) { } } +/// get the cosine of the angle between thrust vector and gravity vector +static int32_t get_vertical_thrust_coeff(void) { + static const int32_t max_bank_coef = BFP_OF_REAL(RadOfDeg(30.), INT32_TRIG_FRAC); + + struct Int32RMat* att = stateGetNedToBodyRMat_i(); + /* thrust vector: + * INT32_RMAT_VMULT(thrust_vect, att, zaxis) + * same as last colum of rmat with INT32_TRIG_FRAC + * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]}; + * + * Angle between two vectors v1 and v2: + * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2))) + * since here both are already of unit length: + * angle = acos(dot(v1, v2)) + * since we we want the cosine of the angle we simply need + * thrust_coeff = dot(v1, v2) + * also can be simplified considering: v1 is zaxis with (0,0,1) + * dot(v1, v2) = v1.z * v2.z = v2.z + */ + int32_t coef = att->m[8]; + if (coef < max_bank_coef) + coef = max_bank_coef; + return coef; +} + #define FF_CMD_FRAC 18 -#define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC)) - -void run_hover_loop(bool_t in_flight) { +static void run_hover_loop(bool_t in_flight) { /* convert our reference to generic representation */ int64_t tmp = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC); @@ -299,14 +331,8 @@ void run_hover_loop(bool_t in_flight) { (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC)); guidance_v_ff_cmd = g_m_zdd / inv_m; - int32_t cphi,ctheta,cphitheta; - struct Int32Eulers* att_euler = stateGetNedToBodyEulers_i(); - PPRZ_ITRIG_COS(cphi, att_euler->phi); - PPRZ_ITRIG_COS(ctheta, att_euler->theta); - cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC; - if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF; /* feed forward command */ - guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta; + guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff; /* bound the nominal command to 0.9*MAX_PPRZ */ Bound(guidance_v_ff_cmd, 0, 8640); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c index 382a3cb0bb..e1cd5e4979 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c @@ -64,7 +64,7 @@ PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE) /** Adapt noise factor. * Smaller values will make the filter to adapt faster. - * Bigger values (slower adaptation) make the filter more robust to external perturbations. + * Bigger values (slower adaptation) make the filter more robust to external pertubations. * Factor should always be >0 */ #ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 060367961d..cb04be854d 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -77,14 +77,14 @@ /* if PRINT_CONFIG is defined, print some config options */ PRINT_CONFIG_VAR(PERIODIC_FREQUENCY) -#ifndef TELEMETRY_FREQUENCY -#define TELEMETRY_FREQUENCY 60 -#endif +/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h + * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file + */ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY) -#ifndef MODULES_FREQUENCY -#define MODULES_FREQUENCY 512 -#endif +/* MODULES_FREQUENCY is defined in generated/modules.h + * according to main_freq parameter set for modules in airframe file + */ PRINT_CONFIG_VAR(MODULES_FREQUENCY) #ifndef BARO_PERIODIC_FREQUENCY @@ -222,6 +222,14 @@ STATIC_INLINE void failsafe_check( void ) { autopilot_set_mode(AP_MODE_FAILSAFE); } +#if FAILSAFE_ON_BAT_CRITICAL + if (autopilot_mode != AP_MODE_KILL && + electrical.bat_critical) + { + autopilot_set_mode(AP_MODE_FAILSAFE); + } +#endif + #if USE_GPS if (autopilot_mode == AP_MODE_NAV && autopilot_motors_on && @@ -233,6 +241,8 @@ STATIC_INLINE void failsafe_check( void ) { autopilot_set_mode(AP_MODE_FAILSAFE); } #endif + + autopilot_check_in_flight(autopilot_motors_on); } STATIC_INLINE void main_event( void ) { @@ -284,6 +294,9 @@ 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(); diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index f78723abbf..366ff9d010 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -336,9 +336,13 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int } bool_t nav_detect_ground(void) { - if (!autopilot_detect_ground) return FALSE; - autopilot_detect_ground = FALSE; + if (!autopilot_ground_detected) return FALSE; + autopilot_ground_detected = FALSE; return TRUE; } +bool_t nav_is_in_flight(void) { + return autopilot_in_flight; +} + void nav_home(void) {} diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index e992f9f311..b8df4ee553 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -81,6 +81,7 @@ unit_t nav_reset_alt( void ) __attribute__ ((unused)); void nav_periodic_task(void); 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); void nav_home(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 7a0572f69e..94b47466e4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -34,7 +34,7 @@ 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_from_eulers_i(struct Int32Eulers *sp_euler); +extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd); extern void stabilization_attitude_run(bool_t in_flight); 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 b2f9f24178..3a5df19417 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -86,8 +86,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); } 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 f435e97a1f..a86f9c4c80 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -102,8 +102,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c index 69f7004aca..5ddcb0e7c3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c @@ -78,7 +78,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); } 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 e546692a87..a5764fa859 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -142,9 +142,74 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_quat.qz = sinf(heading2); } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler); - FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler); +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); + + /* 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); + + /// @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(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); } 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 1bff46cd25..f709f9f5c0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -92,11 +92,85 @@ void stabilization_attitude_set_failsafe_setpoint(void) { PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); } -void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { +void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { // copy euler setpoint for debugging - memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); - INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler); - INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); + + /// @todo calc sp_quat in fixed-point + + /* 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); + + const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi); + + /// @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); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 21c7083013..22a4f8bbcd 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -134,6 +134,28 @@ #define PERIODIC_SEND_PPM(_trans, _dev) {} #endif +#ifdef USE_SUPERBITRF +#include "subsystems/datalink/superbitrf.h" +#define PERIODIC_SEND_SUPERBITRF(_trans, _dev) { \ + DOWNLINK_SEND_SUPERBITRF(_trans, _dev, \ + &superbitrf.status, \ + &superbitrf.cyrf6936.status, \ + &superbitrf.irq_count, \ + &superbitrf.rx_packet_count, \ + &superbitrf.tx_packet_count, \ + &superbitrf.transfer_timeouts, \ + &superbitrf.resync_count, \ + &superbitrf.uplink_count, \ + &superbitrf.rc_count, \ + &superbitrf.timing1, \ + &superbitrf.timing2, \ + &superbitrf.bind_mfg_id32, \ + 6, \ + superbitrf.cyrf6936.mfg_id);} +#else +#define PERIODIC_SEND_SUPERBITRF(_trans, _dev) {} +#endif + #ifdef ACTUATORS #define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, ACTUATORS_NB, actuators) #else @@ -625,6 +647,14 @@ &guidance_v_delta_t); \ } +#define PERIODIC_SEND_TUNE_VERT(_trans, _dev) { \ + DOWNLINK_SEND_TUNE_VERT(_trans, _dev, \ + &guidance_v_z_sp, \ + &ins_ltp_pos.z, \ + &guidance_v_z_ref, \ + &guidance_v_zd_ref); \ + } + #define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) { \ DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ &guidance_h_pos_sp.x, \ @@ -935,34 +965,35 @@ #ifdef ARDRONE2_RAW #include "navdata.h" #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) DOWNLINK_SEND_ARDRONE_NAVDATA(_trans, _dev, \ - &navdata->taille, \ - &navdata->nu_trame, \ - &navdata->ax, \ - &navdata->ay, \ - &navdata->az, \ - &navdata->vx, \ - &navdata->vy, \ - &navdata->vz, \ - &navdata->temperature_acc, \ - &navdata->temperature_gyro, \ - &navdata->ultrasound, \ - &navdata->us_debut_echo, \ - &navdata->us_fin_echo, \ - &navdata->us_association_echo, \ - &navdata->us_distance_echo, \ - &navdata->us_curve_time, \ - &navdata->us_curve_value, \ - &navdata->us_curve_ref, \ - &navdata->nb_echo, \ - &navdata->sum_echo, \ - &navdata->gradient, \ - &navdata->flag_echo_ini, \ - &navdata->pressure, \ - &navdata->temperature_pressure, \ - &navdata->mx, \ - &navdata->my, \ - &navdata->mz, \ - &navdata->chksum \ + &navdata.taille, \ + &navdata.nu_trame, \ + &navdata.ax, \ + &navdata.ay, \ + &navdata.az, \ + &navdata.vx, \ + &navdata.vy, \ + &navdata.vz, \ + &navdata.temperature_acc, \ + &navdata.temperature_gyro, \ + &navdata.ultrasound, \ + &navdata.us_debut_echo, \ + &navdata.us_fin_echo, \ + &navdata.us_association_echo, \ + &navdata.us_distance_echo, \ + &navdata.us_curve_time, \ + &navdata.us_curve_value, \ + &navdata.us_curve_ref, \ + &navdata.nb_echo, \ + &navdata.sum_echo, \ + &navdata.gradient, \ + &navdata.flag_echo_ini, \ + &navdata.pressure, \ + &navdata.temperature_pressure, \ + &navdata.mx, \ + &navdata.my, \ + &navdata.mz, \ + &navdata.chksum, \ + &nav_port.checksum_errors \ ) #else #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {} @@ -977,10 +1008,13 @@ #ifdef USE_UART1 #define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) { \ const uint8_t _bus1 = 1; \ + uint16_t ore = uart1.ore; \ + uint16_t ne_err = uart1.ne_err; \ + uint16_t fe_err = uart1.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart1.ore, \ - &uart1.ne_err, \ - &uart1.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus1); \ } #else @@ -990,10 +1024,13 @@ #ifdef USE_UART2 #define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) { \ const uint8_t _bus2 = 2; \ + uint16_t ore = uart2.ore; \ + uint16_t ne_err = uart2.ne_err; \ + uint16_t fe_err = uart2.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart2.ore, \ - &uart2.ne_err, \ - &uart2.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus2); \ } #else @@ -1003,10 +1040,13 @@ #ifdef USE_UART3 #define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) { \ const uint8_t _bus3 = 3; \ + uint16_t ore = uart3.ore; \ + uint16_t ne_err = uart3.ne_err; \ + uint16_t fe_err = uart3.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart3.ore, \ - &uart3.ne_err, \ - &uart3.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus3); \ } #else @@ -1016,10 +1056,13 @@ #ifdef USE_UART5 #define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) { \ const uint8_t _bus5 = 5; \ + uint16_t ore = uart5.ore; \ + uint16_t ne_err = uart5.ne_err; \ + uint16_t fe_err = uart5.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart5.ore, \ - &uart5.ne_err, \ - &uart5.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus5); \ } #else diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 4bf69fc197..25439b28c5 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -626,6 +626,18 @@ (_ri).r = RATE_BFP_OF_REAL((_rf).r); \ } +#define POSITIONS_FLOAT_OF_BFP(_ef, _ei) { \ + (_ef).x = POS_FLOAT_OF_BFP((_ei).x); \ + (_ef).y = POS_FLOAT_OF_BFP((_ei).y); \ + (_ef).z = POS_FLOAT_OF_BFP((_ei).z); \ + } + +#define POSITIONS_BFP_OF_REAL(_ef, _ei) { \ + (_ef).x = POS_BFP_OF_REAL((_ei).x); \ + (_ef).y = POS_BFP_OF_REAL((_ei).y); \ + (_ef).z = POS_BFP_OF_REAL((_ei).z); \ + } + #define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \ (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \ (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \ diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 413a1c06fc..799087f1f6 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -647,6 +647,22 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { (_q).qz = san * (_uv).z; \ } +#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) { \ + const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \ + if (ov_norm < 1e-8) { \ + (_q).qi = 1; \ + (_q).qx = 0; \ + (_q).qy = 0; \ + (_q).qz = 0; \ + } else { \ + const float s2_normalized = sinf(ov_norm/2.0) / ov_norm; \ + (_q).qi = cosf(ov_norm/2.0); \ + (_q).qx = (_ov).x * s2_normalized; \ + (_q).qy = (_ov).y * s2_normalized; \ + (_q).qz = (_ov).z * s2_normalized; \ + } \ + } + #define FLOAT_QUAT_OF_RMAT(_q, _r) { \ const float tr = RMAT_TRACE((_r)); \ if (tr > 0) { \ diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h index 69df6f8e44..5903506386 100644 --- a/sw/airborne/math/pprz_geodetic_double.h +++ b/sw/airborne/math/pprz_geodetic_double.h @@ -48,8 +48,6 @@ struct EcefCoor_d { /** * @brief vector in Latitude, Longitude and Altitude - * @details Units lat,lon: radians - * Unit alt: meters above MSL */ struct LlaCoor_d { double lon; ///< in radians diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h index 9d021880ad..7d964bf742 100644 --- a/sw/airborne/math/pprz_geodetic_float.h +++ b/sw/airborne/math/pprz_geodetic_float.h @@ -48,8 +48,6 @@ struct EcefCoor_f { /** * @brief vector in Latitude, Longitude and Altitude - * @details Units lat,lon: radians - * Unit alt: meters above MSL */ struct LlaCoor_f { float lon; ///< in radians 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 ddd35318fe..cbeac4ac5f 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -50,8 +50,6 @@ struct EcefCoor_i { /** * @brief vector in Latitude, Longitude and Altitude - * @details Units lat,lon: radians*1e7 - * Unit alt: centimeters above MSL */ struct LlaCoor_i { int32_t lon; ///< in radians*1e7 @@ -106,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); @@ -114,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/mcu_periph/gpio.h b/sw/airborne/mcu_periph/gpio.h index 1218ae6c05..0c500b3dd6 100644 --- a/sw/airborne/mcu_periph/gpio.h +++ b/sw/airborne/mcu_periph/gpio.h @@ -25,6 +25,9 @@ * Some architecture independent helper functions for GPIOs. */ +#ifndef MCU_PERIPH_GPIO_H +#define MCU_PERIPH_GPIO_H + #include "std.h" #include "mcu_periph/gpio_arch.h" @@ -38,3 +41,4 @@ extern void gpio_setup_output(uint32_t port, uint16_t pin); */ extern void gpio_setup_input(uint32_t port, uint16_t pin); +#endif /* MCU_PERIPH_GPIO_H */ diff --git a/sw/airborne/modules/bat_checker/bat_checker.c b/sw/airborne/modules/bat_checker/bat_checker.c index 7949d8bd9b..bdc716cb4d 100644 --- a/sw/airborne/modules/bat_checker/bat_checker.c +++ b/sw/airborne/modules/bat_checker/bat_checker.c @@ -19,34 +19,24 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file modules/bat_checker/bat_checker.c + * + * Activate a buzzer/LED periodically or periodically to warn of low/critical battery level. + * At LOW_BAT_LEVEL the buzzer will be activated periodically. + * At CRITIC_BAT_LEVEL the buzzer will be activated permanently. + */ + #include "bat_checker.h" #include "generated/airframe.h" #include "generated/modules.h" #include "subsystems/electrical.h" #include "led.h" -#ifndef CRITIC_BAT_LEVEL -#error You must define CRITIC_BAT_LEVEL to use this module! -#endif - -#ifndef LOW_BAT_LEVEL -#error You must define LOW_BAT_LEVEL to use this module! -#endif - #ifndef BAT_CHECKER_LED #error You must define BAT_CHECKER_LED in your airframe file. #endif -#ifndef BAT_CHECKER_DELAY -#define BAT_CHECKER_DELAY 5 -#endif -PRINT_CONFIG_VAR(BAT_CHECKER_DELAY) - -// at this level, the buzzer will be activated periodically -#define WARN_BAT_LEVEL1 (LOW_BAT_LEVEL*10) - -// at this level, the buzzer will be activated permanently -#define WARN_BAT_LEVEL2 (CRITIC_BAT_LEVEL*10) void init_bat_checker(void) { LED_INIT(BAT_CHECKER_LED); @@ -54,22 +44,12 @@ void init_bat_checker(void) { } void bat_checker_periodic(void) { - static uint8_t bat_low_counter = 0; - if(electrical.vsupply < WARN_BAT_LEVEL1) { - if(bat_low_counter) - bat_low_counter--; - } else { - bat_low_counter = BAT_CHECKER_DELAY * BAT_CHECKER_PERIODIC_FREQ; - } - if(!bat_low_counter) { - if(electrical.vsupply < WARN_BAT_LEVEL2) { - LED_ON(BAT_CHECKER_LED); - } else if(electrical.vsupply < WARN_BAT_LEVEL1) { - LED_TOGGLE(BAT_CHECKER_LED); - } - } else { + if (electrical.bat_critical) + LED_ON(BAT_CHECKER_LED); + else if (electrical.bat_low) + LED_TOGGLE(BAT_CHECKER_LED); + else LED_OFF(BAT_CHECKER_LED); - } -} +} diff --git a/sw/airborne/modules/bat_checker/bat_checker.h b/sw/airborne/modules/bat_checker/bat_checker.h index 77c07fe41d..5a3304e9fa 100644 --- a/sw/airborne/modules/bat_checker/bat_checker.h +++ b/sw/airborne/modules/bat_checker/bat_checker.h @@ -19,6 +19,14 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file modules/bat_checker/bat_checker.c + * + * Activate a buzzer/LED periodically or periodically to warn of low/critical battery level. + * At LOW_BAT_LEVEL the buzzer will be activated periodically. + * At CRITIC_BAT_LEVEL the buzzer will be activated permanently. + */ + #ifndef BAT_CHECKER_H #define BAT_CHECKER_H 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/airspeed_adc.c b/sw/airborne/modules/sensors/airspeed_adc.c index f461d310bf..af9955437e 100644 --- a/sw/airborne/modules/sensors/airspeed_adc.c +++ b/sw/airborne/modules/sensors/airspeed_adc.c @@ -60,7 +60,7 @@ void airspeed_adc_update( void ) { float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS); #endif stateSetAirspeed_f(&airspeed); -#else // SITL +#elif !defined USE_NPS extern float sim_air_speed; stateSetAirspeed_f(&sim_air_speed); adc_airspeed_val = 0; diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c index 9d92ebac7f..abf946bd96 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -109,9 +109,13 @@ void airspeed_amsys_read_periodic( void ) { #endif } -#elif !USE_NPS - extern float sim_air_speed; - stateSetAirspeed_f(&sim_air_speed); +#if USE_AIRSPEED + stateSetAirspeed_f(&airspeed_amsys); +#endif + +#elif !defined USE_NPS + extern float sim_air_speed; + stateSetAirspeed_f(&sim_air_speed); #endif //SITL diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index a6c78cd08b..6f803c30b3 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -128,7 +128,7 @@ void airspeed_ets_read_periodic( void ) { } if (airspeed_ets_i2c_trans.status == I2CTransDone) i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2); -#else // SITL +#elif !defined USE_NPS extern float sim_air_speed; stateSetAirspeed_f(&sim_air_speed); #endif //SITL diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index d3b3a3d88c..e44e3044f8 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -72,17 +72,10 @@ void baro_ms5611_periodic_check( void ) { ms5611_i2c_periodic_check(&baro_ms5611); - if (baro_ms5611.initialized) { - 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])); - } +#if BARO_MS5611_SEND_COEFF + // send coeff every 5s + RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#endif } /// trigger new measurement or initialize if needed @@ -110,3 +103,17 @@ void baro_ms5611_event( void ) { #endif } } + +void baro_ms5611_send_coeff(void) { + if (baro_ms5611.initialized) { + 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]); + } +} diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h index f30b28debc..549b6bf731 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -21,6 +21,7 @@ extern void baro_ms5611_init(void); extern void baro_ms5611_read(void); 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; } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index c91d713153..7da2575f41 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -72,17 +72,11 @@ void baro_ms5611_periodic_check( void ) { ms5611_spi_periodic_check(&baro_ms5611); - if (baro_ms5611.initialized) { - 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])); - } +#if BARO_MS5611_SEND_COEFF + // send coeff every 5s + RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#endif + } /// trigger new measurement or initialize if needed @@ -110,3 +104,17 @@ void baro_ms5611_event( void ) { #endif } } + +void baro_ms5611_send_coeff(void) { + if (baro_ms5611.initialized) { + 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]); + } +} diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h index 7bab9d92e8..7be9f9c2e5 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.h +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h @@ -49,9 +49,10 @@ extern void baro_ms5611_init(void); extern void baro_ms5611_read(void); extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); +extern void baro_ms5611_send_coeff(void); -#define BaroMs5611UpdatePressure(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; baro_ms5611.data_available = FALSE; } } +#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } -#define BaroMs5611UpdateAlt(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; 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/time/flight_time.c b/sw/airborne/modules/time/flight_time.c new file mode 100644 index 0000000000..16328c7c84 --- /dev/null +++ b/sw/airborne/modules/time/flight_time.c @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2013 Elisabeth van der Sman, 2013 Freek van Tienen + * + * 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/time/flight_time.c + * + * Flight time counter that can be set from the gcs + */ + +#include "flight_time.h" +#include "generated/airframe.h" + +uint16_t time_until_land; + +#ifndef FLIGHT_TIME_LEFT +#define FLIGHT_TIME_LEFT 10000 +#endif + +void flight_time_init(void) { + time_until_land = FLIGHT_TIME_LEFT; +} + +void flight_time_periodic( void ) { + // Count downwards + if(time_until_land > 0) + time_until_land--; +} diff --git a/sw/airborne/modules/time/flight_time.h b/sw/airborne/modules/time/flight_time.h new file mode 100644 index 0000000000..2a4ff41f89 --- /dev/null +++ b/sw/airborne/modules/time/flight_time.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2013 Elisabeth van der Sman, 2013 Freek van Tienen + * + * 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/time/flight_time.h + * + * Flight time counter that can be set from the gcs + */ + +#ifndef FLIGHT_TIME_H +#define FLIGHT_TIME_H + +#include "std.h" + +extern uint16_t time_until_land; + +void flight_time_init(void); +void flight_time_periodic(void); + +#endif /* FLIGHT_TIME_H */ diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c new file mode 100644 index 0000000000..1b1e678a48 --- /dev/null +++ b/sw/airborne/peripherals/cyrf6936.c @@ -0,0 +1,420 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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 peripherals/cyrf6936.c + * Driver for the cyrf6936 2.4GHz radio chip + */ + +#include "cyrf6936.h" +#include "mcu_periph/spi.h" +#include "mcu_periph/gpio.h" +#include "mcu_periph/sys_time.h" +#include "subsystems/radio_control.h" + +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + +/* Static functions used in the different statuses */ +static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data); +static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length); +static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr); +static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length); + +/** + * Initializing the cyrf chip + */ +void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin) { + /* Set spi_peripheral and the status */ + cyrf->spi_p = spi_p; + cyrf->status = CYRF6936_UNINIT; + cyrf->has_irq = FALSE; + + /* Set the spi transaction */ + cyrf->spi_t.cpol = SPICpolIdleLow; + cyrf->spi_t.cpha = SPICphaEdge1; + cyrf->spi_t.dss = SPIDss8bit; + cyrf->spi_t.bitorder = SPIMSBFirst; + cyrf->spi_t.cdiv = SPIDiv64; + + cyrf->spi_t.input_length = 0; + cyrf->spi_t.output_length = 0; + cyrf->spi_t.input_buf = cyrf->input_buf; + cyrf->spi_t.output_buf = cyrf->output_buf; + cyrf->spi_t.slave_idx = slave_idx; + cyrf->spi_t.select = SPISelectUnselect; + cyrf->spi_t.status = SPITransDone; + + /* Reset the CYRF6936 chip (busy waiting) */ + gpio_setup_output(rst_port, rst_pin); + gpio_set(rst_port, rst_pin); + sys_time_usleep(100); + gpio_clear(rst_port, rst_pin); + sys_time_usleep(100); + + /* Get the MFG ID */ + cyrf->status = CYRF6936_GET_MFG_ID; + cyrf->buffer_idx = 0; + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); +} + +/** + * The on event call for the CYRF6936 chip + */ +void cyrf6936_event(struct Cyrf6936 *cyrf) { + int i; + // Check if cyrf is initialized + if(cyrf->status == CYRF6936_UNINIT) + return; + + // Check if there is still a transaction in progress + if(cyrf->spi_t.status == SPITransPending || cyrf->spi_t.status == SPITransRunning) + return; + + /* Check the status of the cyrf */ + switch (cyrf->status) { + + /* Getting the MFG id */ + case CYRF6936_GET_MFG_ID: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); + break; + case 1: + cyrf6936_read_block(cyrf, CYRF_MFG_ID, 6); + break; + case 2: + // Copy the MFG id + for(i = 0; i < 6; i++) + cyrf->mfg_id[i] = cyrf->input_buf[i+1]; + + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0x00); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* Do a multi write */ + case CYRF6936_MULTIWRITE: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // When we are done writing + if(cyrf->buffer_idx == cyrf->buffer_length) { + cyrf->status = CYRF6936_IDLE; + break; + } + + // Write the next register from the buffer + cyrf6936_write_register(cyrf, + ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][0], + ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][1]); + break; + + /* Do a write of the data code */ + case CYRF6936_DATA_CODE: + break; + + /* Do a write of channel, sop, data and crc */ + case CYRF6936_CHAN_SOP_DATA_CRC: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Write the CRC LSB + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]); + break; + case 1: // Write the CRC MSB + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_MSB, cyrf->buffer[1]); + break; + case 2: // Write the SOP code + cyrf6936_write_block(cyrf, CYRF_SOP_CODE, &(cyrf->buffer[2]), 8); + break; + case 3: // Write the DATA code + cyrf6936_write_block(cyrf, CYRF_DATA_CODE, &(cyrf->buffer[10]), 16); + break; + case 4: // Write the Channel + cyrf6936_write_register(cyrf, CYRF_CHANNEL, cyrf->buffer[26]); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* Do a read of the receive irq status, receive status and the receive packet */ + case CYRF6936_RX_IRQ_STATUS_PACKET: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Read the receive IRQ status + cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); + break; + case 1: // Read the send IRQ status + cyrf->rx_irq_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_TX_IRQ_STATUS); + break; + case 2: // Read the receive status + cyrf->tx_irq_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_RX_STATUS); + break; + case 3: // Set the packet length + cyrf->rx_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_RX_COUNT); + break; + case 4: // Read the receive packet + cyrf->rx_count = cyrf->input_buf[1]; + cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16); + break; + default: + // Copy the receive packet + for(i = 0; i < 16; i++) + cyrf->rx_packet[i] = cyrf->input_buf[i+1]; + + cyrf->has_irq = TRUE; + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* The CYRF6936 is busy sending a packet */ + case CYRF6936_SEND: + // When the last transaction isn't failed send the next + if(cyrf->spi_t.status != SPITransFailed) + cyrf->buffer_idx++; + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Set the packet length + cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]); + break; + case 1: // Clear the TX buffer + cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_CLR); + break; + case 2: // Write the send packet + cyrf6936_write_block(cyrf, CYRF_TX_BUFFER, &cyrf->buffer[1], 16); + break; + case 3: // Send the packet + cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN | CYRF_TXE_IRQEN); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* This should not happen */ + default: + break; + } +} + +/** + * Write a byte to a register + */ +static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) { + return cyrf6936_write_block(cyrf, addr, &data, 1); +} + +/** + * Write multiple bytes to a register + */ +static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length) { + uint8_t i; + /* Check if there is already a SPI transaction busy */ + if(cyrf->spi_t.status != SPITransDone) + return FALSE; + + /* Set the buffer and commit the transaction */ + cyrf->spi_t.output_length = length+1; + cyrf->spi_t.input_length = 0; + cyrf->output_buf[0] = addr | CYRF_DIR; + + // Copy the data + for(i = 0; i < length; i++) + cyrf->output_buf[i+1] = data[i]; + + // Submit the transaction + return spi_submit(cyrf->spi_p, &(cyrf->spi_t)); +} + +/** + * Read a byte from a register + */ +static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr) { + return cyrf6936_read_block(cyrf, addr, 1); +} + +/** + * Read multiple bytes from a register + */ +static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length) { + if(cyrf->spi_t.status != SPITransDone) + return FALSE; + + /* Set the buffer and commit the transaction */ + cyrf->spi_t.output_length = 1; + cyrf->spi_t.input_length = length + 1; + cyrf->output_buf[0] = addr; + + // Submit the transaction + return spi_submit(cyrf->spi_p, &(cyrf->spi_t)); +} + +/** + * Write to one register + */ +bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) { + const uint8_t data_multi[][2] = { + {addr, data} + }; + return cyrf6936_multi_write(cyrf, data_multi, 1); +} + +/** + * Write to multiple registers one byte + */ +bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length) { + uint8_t i; + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_MULTIWRITE; + + /* Set the multi write */ + cyrf->buffer_length = length; + cyrf->buffer_idx = 0; + + // Copy the buffer + for(i = 0; i< length; i++) { + cyrf->buffer[i*2] = data[i][0]; + cyrf->buffer[i*2+1] = data[i][1]; + } + + /* Write the first regiter */ + if(length > 0) + cyrf6936_write_register(cyrf, data[0][0], data[0][1]); + + return TRUE; +} + +/** + * Set the channel, SOP code, DATA code and the CRC seed + */ +bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed) { + uint8_t i; + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_CHAN_SOP_DATA_CRC; + + // Copy the CRC + cyrf->buffer[0] = crc_seed & 0xFF; + cyrf->buffer[1] = (crc_seed >> 8) & 0xFF; + + // Copy the SOP code + for(i = 0; i < 8; i++) + cyrf->buffer[i+2] = sop_code[i]; + + // Copy the DATA code + for(i = 0; i < 16; i++) + cyrf->buffer[i+10] = data_code[i]; + + // Copy the channel + cyrf->buffer[26] = chan; + + /* Try to write the CRC LSB */ + cyrf->buffer_idx = 0; + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]); + + return TRUE; +} + +/** + * Read the RX IRQ status register, the rx status register and the rx packet + */ +bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) { + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_RX_IRQ_STATUS_PACKET; + + /* Try to read the RX status */ + cyrf->buffer_idx = 0; + cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); + + return TRUE; +} + +/** + * Send a packet with a certain length + */ +bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length) { + int i; + + /* Check if the cyrf6936 isn't busy */ + if(cyrf->status != CYRF6936_IDLE) + return FALSE; + + // Set the status + cyrf->status = CYRF6936_SEND; + + // Copy the length and the data + cyrf->buffer[0] = length; + for(i = 0; i < length; i++) + cyrf->buffer[i+1] = data[i]; + + /* Try to set the packet length */ + cyrf->buffer_idx = 0; + cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]); + + return TRUE; +} diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h new file mode 100644 index 0000000000..c384a9878b --- /dev/null +++ b/sw/airborne/peripherals/cyrf6936.h @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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 peripherals/cyrf6936.h + * Driver for the cyrf6936 2.4GHz radio chip + */ + +#ifndef CYRF6936_H +#define CYRF6936_H + +#include "cyrf6936_regs.h" +#include "mcu_periph/spi.h" + +#define CYRF6936_MAX_BUFFER 80 /**< The max buffer size in the cyrf6936 structure */ + +/* The different statuses the cyrf6936 chip can be in */ +enum Cyrf6936Status { + CYRF6936_UNINIT, /**< The chip isn't initialized */ + CYRF6936_IDLE, /**< The chip is idle and can be used */ + CYRF6936_GET_MFG_ID, /**< The chip is busy with getting the manufacturer ID */ + CYRF6936_MULTIWRITE, /**< The chip is writing multiple registers */ + CYRF6936_DATA_CODE, /**< The chip is writing a data code */ + CYRF6936_CHAN_SOP_DATA_CRC, /**< The chip is setting the channel, SOP code, DATA code and the CRC seed */ + CYRF6936_RX_IRQ_STATUS_PACKET, /**< The chip is getting the receive irq status, receive status and the receive packet */ + CYRF6936_SEND /**< The chip is busy sending a packet */ +}; + +/* The structure for the cyrf6936 chip that handles all the buffers and requests */ +struct Cyrf6936 { + struct spi_periph *spi_p; /**< The SPI peripheral for the connection */ + struct spi_transaction spi_t; /**< The SPI transaction used for the writing and reading of registers */ + volatile enum Cyrf6936Status status; /**< The status of the CYRF6936 chip */ + uint8_t input_buf[17]; /**< The input buffer for the SPI transaction */ + uint8_t output_buf[17]; /**< The output buffer for the SPI transaction */ + + uint8_t buffer[CYRF6936_MAX_BUFFER]; /**< The buffer used to write/read multiple registers */ + uint8_t buffer_length; /**< The length of the buffer used for MULTIWRITE */ + uint8_t buffer_idx; /**< The index of the buffer used for MULTIWRITE and used as sub-status for other statuses */ + + bool_t has_irq; /**< When the CYRF6936 is done reading the irq */ + uint8_t mfg_id[6]; /**< The manufacturer id of the CYRF6936 chip */ + uint8_t tx_irq_status; /**< The last send interrupt status */ + uint8_t rx_irq_status; /**< The last receive interrupt status */ + uint8_t rx_status; /**< The last receive status */ + uint8_t rx_count; /**< The length of the received packet */ + uint8_t rx_packet[16]; /**< The last received packet */ +}; + +extern void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin); +void cyrf6936_event(struct Cyrf6936 *cyrf); + +bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data); +bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length); +bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed); +bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf); +bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length); + +#endif /* CYRF6936_H */ diff --git a/sw/airborne/peripherals/cyrf6936_regs.h b/sw/airborne/peripherals/cyrf6936_regs.h new file mode 100644 index 0000000000..c5926771a7 --- /dev/null +++ b/sw/airborne/peripherals/cyrf6936_regs.h @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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 peripherals/cyrf6936_regs.h + * Register defines for the CYRF6936 2.4GHz radio chip + */ + +#ifndef CYRF6936_REGS_H +#define CYRF6936_REGS_H + +/* The SPI interface defines */ +enum { + CYRF_CHANNEL = 0x00, + CYRF_TX_LENGTH = 0x01, + CYRF_TX_CTRL = 0x02, + CYRF_TX_CFG = 0x03, + CYRF_TX_IRQ_STATUS = 0x04, + CYRF_RX_CTRL = 0x05, + CYRF_RX_CFG = 0x06, + CYRF_RX_IRQ_STATUS = 0x07, + CYRF_RX_STATUS = 0x08, + CYRF_RX_COUNT = 0x09, + CYRF_RX_LENGTH = 0x0A, + CYRF_PWR_CTRL = 0x0B, + CYRF_XTAL_CTRL = 0x0C, + CYRF_IO_CFG = 0x0D, + CYRF_GPIO_CTRL = 0x0E, + CYRF_XACT_CFG = 0x0F, + CYRF_FRAMING_CFG = 0x10, + CYRF_DATA32_THOLD = 0x11, + CYRF_DATA64_THOLD = 0x12, + CYRF_RSSI = 0x13, + CYRF_EOP_CTRL = 0x14, + CYRF_CRC_SEED_LSB = 0x15, + CYRF_CRC_SEED_MSB = 0x16, + CYRF_TX_CRC_LSB = 0x17, + CYRF_TX_CRC_MSB = 0x18, + CYRF_RX_CRC_LSB = 0x19, + CYRF_RX_CRC_MSB = 0x1A, + CYRF_TX_OFFSET_LSB = 0x1B, + CYRF_TX_OFFSET_MSB = 0x1C, + CYRF_MODE_OVERRIDE = 0x1D, + CYRF_RX_OVERRIDE = 0x1E, + CYRF_TX_OVERRIDE = 0x1F, + CYRF_TX_BUFFER = 0x20, + CYRF_RX_BUFFER = 0x21, + CYRF_SOP_CODE = 0x22, + CYRF_DATA_CODE = 0x23, + CYRF_PREAMBLE = 0x24, + CYRF_MFG_ID = 0x25, + CYRF_XTAL_CFG = 0x26, + CYRF_CLK_OFFSET = 0x27, + CYRF_CLK_EN = 0x28, + CYRF_RX_ABORT = 0x29, + CYRF_AUTO_CAL_TIME = 0x32, + CYRF_AUTO_CAL_OFFSET = 0x35, + CYRF_ANALOG_CTRL = 0x39, +}; +#define CYRF_DIR (1<<7) /**< Bit for enabling writing */ + +// CYRF_MODE_OVERRIDE +#define CYRF_RST (1<<0) + +// CYRF_CLK_EN +#define CYRF_RXF (1<<1) + +// CYRF_XACT_CFG +enum { + CYRF_MODE_SLEEP = (0x0 <<2), + CYRF_MODE_IDLE = (0x1 <<2), + CYRF_MODE_SYNTH_TX = (0x2 <<2), + CYRF_MODE_SYNTH_RX = (0x3 <<2), + CYRF_MODE_RX = (0x4 <<2), +}; +#define CYRF_FRC_END (1<<5) +#define CYRF_ACK_EN (1<<7) + +// CYRF_IO_CFG +#define CYRF_IRQ_GPIO (1<<0) +#define CYRF_SPI_3PIN (1<<1) +#define CYRF_PACTL_GPIO (1<<2) +#define CYRF_PACTL_OD (1<<3) +#define CYRF_XOUT_OD (1<<4) +#define CYRF_MISO_OD (1<<5) +#define CYRF_IRQ_POL (1<<6) +#define CYRF_IRQ_OD (1<<7) + +// CYRF_FRAMING_CFG +#define CYRF_LEN_EN (1<<5) +#define CYRF_SOP_LEN (1<<6) +#define CYRF_SOP_EN (1<<7) + +// CYRF_RX_STATUS +enum { + CYRF_RX_DATA_MODE_GFSK = 0x00, + CYRF_RX_DATA_MODE_8DR = 0x01, + CYRF_RX_DATA_MODE_DDR = 0x10, + CYRF_RX_DATA_MODE_NV = 0x11, +}; +#define CYRF_RX_CODE (1<<2) +#define CYRF_BAD_CRC (1<<3) +#define CYRF_CRC0 (1<<4) +#define CYRF_EOP_ERR (1<<5) +#define CYRF_PKT_ERR (1<<6) +#define CYRF_RX_ACK (1<<7) + +// CYRF_TX_IRQ_STATUS +#define CYRF_TXE_IRQ (1<<0) +#define CYRF_TXC_IRQ (1<<1) +#define CYRF_TXBERR_IRQ (1<<2) +#define CYRF_TXB0_IRQ (1<<3) +#define CYRF_TXB8_IRQ (1<<4) +#define CYRF_TXB15_IRQ (1<<5) +#define CYRF_LV_IRQ (1<<6) +#define CYRF_OS_IRQ (1<<7) + +// CYRF_RX_IRQ_STATUS +#define CYRF_RXE_IRQ (1<<0) +#define CYRF_RXC_IRQ (1<<1) +#define CYRF_RXBERR_IRQ (1<<2) +#define CYRF_RXB1_IRQ (1<<3) +#define CYRF_RXB8_IRQ (1<<4) +#define CYRF_RXB16_IRQ (1<<5) +#define CYRF_SOPDET_IRQ (1<<6) +#define CYRF_RXOW_IRQ (1<<7) + +// CYRF_TX_CTRL +#define CYRF_TXE_IRQEN (1<<0) +#define CYRF_TXC_IRQEN (1<<1) +#define CYRF_TXBERR_IRQEN (1<<2) +#define CYRF_TXB0_IRQEN (1<<3) +#define CYRF_TXB8_IRQEN (1<<4) +#define CYRF_TXB15_IRQEN (1<<5) +#define CYRF_TX_CLR (1<<6) +#define CYRF_TX_GO (1<<7) + +// CYRF_RX_CTRL +#define CYRF_RXE_IRQEN (1<<0) +#define CYRF_RXC_IRQEN (1<<1) +#define CYRF_RXBERR_IRQEN (1<<2) +#define CYRF_RXB1_IRQEN (1<<3) +#define CYRF_RXB8_IRQEN (1<<4) +#define CYRF_RXB16_IRQEN (1<<5) +#define CYRF_RSVD (1<<6) +#define CYRF_RX_GO (1<<7) + +// CYRF_RX_OVERRIDE +#define CYRF_ACE (1<<1) +#define CYRF_DIS_RXCRC (1<<2) +#define CYRF_DIS_CRC0 (1<<3) +#define CYRF_FRC_RXDR (1<<4) +#define CYRF_MAN_RXACK (1<<5) +#define CYRF_RXTX_DLY (1<<6) +#define CYRF_ACK_RX (1<<7) + +// CYRF_TX_OVERRIDE +#define CYRF_TX_INV (1<<0) +#define CYRF_DIS_TXCRC (1<<2) +#define CYRF_OVRD_ACK (1<<3) +#define CYRF_MAN_TXACK (1<<4) +#define CYRF_FRC_PRE (1<<6) +#define CYRF_ACK_TX (1<<7) + +// CYRF_RX_CFG +#define CYRF_VLD_EN (1<<0) +#define CYRF_RXOW_EN (1<<1) +#define CYRF_FAST_TURN_EN (1<<3) +#define CYRF_HILO (1<<4) +#define CYRF_ATT (1<<5) +#define CYRF_LNA (1<<6) +#define CYRF_AGC_EN (1<<7) + +// CYRF_TX_CFG +enum { + CYRF_PA_M35 = 0x0, + CYRF_PA_M30 = 0x1, + CYRF_PA_M24 = 0x2, + CYRF_PA_M18 = 0x3, + CYRF_PA_M13 = 0x4, + CYRF_PA_M5 = 0x5, + CYRF_PA_0 = 0x6, + CYRF_PA_4 = 0x7, +}; +enum { + CYRF_DATA_MODE_GFSK = (0x0 <<3), + CYRF_DATA_MODE_8DR = (0x1 <<3), + CYRF_DATA_MODE_DDR = (0x2 <<3), + CYRF_DATA_MODE_SDR = (0x3 <<3), +}; +#define CYRF_DATA_CODE_LENGTH (1<<5) + +#endif // CYRF6936_REGS_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/state.h b/sw/airborne/state.h index 9818c0fafd..ee1436f1c4 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -148,7 +148,7 @@ struct State { /** * Position in Latitude, Longitude and Altitude. * Units lat,lon: radians*1e7 - * Units alt: centimeters above MSL + * Units alt: milimeters above reference ellipsoid */ struct LlaCoor_i lla_pos_i; @@ -196,7 +196,7 @@ struct State { /** * Position in Latitude, Longitude and Altitude. * Units lat,lon: radians - * Units alt: meters above MSL + * Units alt: meters above reference ellipsoid */ struct LlaCoor_f lla_pos_f; @@ -487,12 +487,12 @@ extern void stateCalcPositionLla_f(void); /// Test if local coordinates are valid. static inline bool_t stateIsLocalCoordinateValid(void) { - return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status & ~(POS_LOCAL_COORD))); + return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status & (POS_LOCAL_COORD))); } /// Test if global coordinates are valid. static inline bool_t stateIsGlobalCoordinateValid(void) { - return ((state.pos_status & ~(POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid()); + return ((state.pos_status & (POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid()); } /************************ Set functions ****************************/ diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index afed7a6202..1e5317e937 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -27,6 +27,11 @@ * and also sets battery level. */ +#ifdef ARDRONE2_DEBUG +# include +# include +#endif + #include "ahrs_ardrone2.h" #include "state.h" #include "math/pprz_algebra_float.h" @@ -55,15 +60,42 @@ void ahrs_align(void) { } +#ifdef ARDRONE2_DEBUG +static void dump(const void *_b, size_t s) { + const unsigned char *b = _b; + size_t n; + + for(n = 0; n < s; ++n) { + printf("%02x ", b[n]); + if (n%16 == 15) + printf("\n"); + } + if (n%16 != 0) + printf("\n"); +} +#endif + void ahrs_propagate(void) { + int l; + //Recieve the main packet - at_com_recieve_navdata(buffer); + l = at_com_recieve_navdata(buffer); navdata_t* main_packet = (navdata_t*) &buffer; +#ifdef ARDRONE2_DEBUG + if (l < 0) + printf("errno = %d\n", errno); +#endif + //When this isn't a valid packet return - if(main_packet->header != NAVDATA_HEADER) + if(l < 0 || main_packet->header != NAVDATA_HEADER) return; +#ifdef ARDRONE2_DEBUG + printf("Read %d\n", l); + dump(buffer, l); +#endif + //Set the state ahrs_impl.state = main_packet->ardrone_state; @@ -78,6 +110,9 @@ void ahrs_propagate(void) { //Read the navdata until packet is fully readed while(!full_read && navdata_option->size > 0) { +#ifdef ARDRONE2_DEBUG + printf ("tag = %d\n", navdata_option->tag); +#endif //Check the tag for the right option switch(navdata_option->tag) { case 0: //NAVDATA_DEMO @@ -112,6 +147,9 @@ void ahrs_propagate(void) { break; #ifdef USE_GPS_ARDRONE2 case 27: //NAVDATA_GPS +# ifdef ARDRONE2_DEBUG + dump(navdata_option, navdata_option->size); +# endif navdata_gps = (navdata_gps_t*) navdata_option; // Send the data to the gps parser @@ -123,7 +161,9 @@ void ahrs_propagate(void) { full_read = TRUE; break; default: - //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); +#ifdef ARDRONE2_DEBUG + printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); +#endif break; } navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size); diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index 1259ba9ff1..ad1f8bb08c 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -46,6 +46,7 @@ #define PPRZ 1 #define XBEE 2 #define UDP 3 +#define SUPERBITRF 4 EXTERN bool_t dl_msg_available; /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/ @@ -90,10 +91,16 @@ EXTERN void dl_parse_msg(void); #elif defined DATALINK && DATALINK == UDP #define DatalinkEvent() { \ - UdpCheckAndParse(); \ + UdpCheckAndParse(); \ DlCheckAndParse(); \ } +#elif defined DATALINK && DATALINK == SUPERBITRF + +#define DatalinkEvent() { \ + SuperbitRFCheckAndParse(); \ + DlCheckAndParse(); \ + } #else diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 39883053b6..851414b494 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -46,16 +46,21 @@ #endif #else /** SITL */ + #include "subsystems/datalink/udp.h" #include "subsystems/datalink/pprz_transport.h" #include "subsystems/datalink/xbee.h" #include "subsystems/datalink/w5100.h" +#if USE_SUPERBITRF +#include "subsystems/datalink/superbitrf.h" +#endif #if USE_AUDIO_TELEMETRY #include "subsystems/datalink/audio_telemetry.h" #endif #ifdef USE_USB_SERIAL #include "mcu_periph/usb_serial.h" #endif + #endif /** !SITL */ #ifndef DefaultChannel diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c new file mode 100644 index 0000000000..fc8efbb801 --- /dev/null +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -0,0 +1,978 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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/datalink/superbitrf.c + * DSM2 and DSMX datalink implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#include "subsystems/datalink/superbitrf.h" + +#include +#include "paparazzi.h" +#include "mcu_periph/spi.h" +#include "mcu_periph/sys_time.h" +#include "mcu_periph/gpio.h" + +/* Default SuperbitRF SPI DEV */ +#ifndef SUPERBITRF_SPI_DEV +#define SUPERBITRF_SPI_DEV spi1 +#endif +PRINT_CONFIG_VAR(SUPERBITRF_SPI_DEV); + +/* Default SuperbitRF RST PORT and PIN */ +#ifndef SUPERBITRF_RST_PORT +#define SUPERBITRF_RST_PORT GPIOC +#endif +PRINT_CONFIG_VAR(SUPERBITRF_RST_PORT); +#ifndef SUPERBITRF_RST_PIN +#define SUPERBITRF_RST_PIN GPIO12 +#endif +PRINT_CONFIG_VAR(SUPERBITRF_RST_PIN); + +/* Default SuperbitRF DRDY(IRQ) PORT and PIN */ +#ifndef SUPERBITRF_DRDY_PORT +#define SUPERBITRF_DRDY_PORT GPIOB +#endif +PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PORT); +#ifndef SUPERBITRF_DRDY_PIN +#define SUPERBITRF_DRDY_PIN GPIO1 +#endif +PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PIN); + +/* Default forcing in DSM2 mode is false */ +#ifndef SUPERBITRF_FORCE_DSM2 +#define SUPERBITRF_FORCE_DSM2 TRUE +#endif +PRINT_CONFIG_VAR(SUPERBITRF_FORCE_DSM2); + +/* The superbitRF structure */ +struct SuperbitRF superbitrf; + +/* The internal functions */ +static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels); +static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]); +static inline void superbitrf_send_packet_cb(bool_t error); +static inline void superbitrf_gen_dsmx_channels(void); + +/* The startup configuration for the cyrf6936 */ +static const uint8_t cyrf_stratup_config[][2] = { + {CYRF_MODE_OVERRIDE, CYRF_RST}, // Reset the device + {CYRF_CLK_EN, CYRF_RXF}, // Enable the clock + {CYRF_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization + {CYRF_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization + {CYRF_RX_CFG, CYRF_LNA | CYRF_FAST_TURN_EN}, // Enable low noise amplifier and fast turning + {CYRF_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration + {CYRF_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, // Force in Synth RX mode + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF_DATA64_THOLD, 0x0E}, // From manual, typical configuration + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX}, // Set in Synth RX mode (again, really needed?) +}; +/* The binding configuration for the cyrf6936 */ +static const uint8_t cyrf_bind_config[][2] = { + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF_FRAMING_CFG, CYRF_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE + {CYRF_RX_OVERRIDE, CYRF_FRC_RXDR | CYRF_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker + {CYRF_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2 + {CYRF_TX_OVERRIDE, CYRF_DIS_TXCRC}, // Disable transmit CRC generate +}; +/* The transfer configuration for the cyrf6936 */ +static const uint8_t cyrf_transfer_config[][2] = { + {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_8DR | CYRF_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm + {CYRF_FRAMING_CFG, CYRF_SOP_EN | CYRF_SOP_LEN | CYRF_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE + {CYRF_TX_OVERRIDE, 0x00}, // Reset TX overrides + {CYRF_RX_OVERRIDE, 0x00}, // Reset RX overrides +}; +/* Abort the receive of the cyrf6936 */ +const uint8_t cyrf_abort_receive[][2] = { + {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, + {CYRF_RX_ABORT, 0x00} +}; +/* Start the receive of the cyrf6936 */ +const uint8_t cyrf_start_receive[][2] = { + {CYRF_RX_IRQ_STATUS, CYRF_RXOW_IRQ}, // Clear the IRQ + {CYRF_RX_CTRL, CYRF_RX_GO | CYRF_RXC_IRQEN | CYRF_RXE_IRQEN} // Start receiving and set the IRQ +}; + +/* The PN codes used for DSM2 and DSMX channel hopping */ +static const uint8_t pn_codes[5][9][8] = { +{ /* Row 0 */ + /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, + /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, + /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, + /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, + /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, + /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, + /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, + /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, + /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} +}, +{ /* Row 1 */ + /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, + /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, + /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, + /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, + /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, + /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, + /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, + /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, + /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} +}, +{ /* Row 2 */ + /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, + /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, + /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, + /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, + /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, + /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, + /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, + /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, + /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} +}, +{ /* Row 3 */ + /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, + /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, + /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, + /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, + /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, + /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, + /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, + /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, + /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40} +}, +{ /* Row 4 */ + /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, + /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, + /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, + /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, + /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, + /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, + /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, + /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, + /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} +}, +}; +static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x84 }; + +/** + * Initialize the superbitrf + */ +void superbitrf_init(void) { + // Set the status to uninitialized and set the timer to 0 + superbitrf.status = SUPERBITRF_UNINIT; + superbitrf.state = 0; + superbitrf.timer = 0; + superbitrf.rx_packet_count = 0; + superbitrf.tx_packet_count = 0; + + // Setup the transmit buffer + superbitrf.tx_insert_idx = 0; + superbitrf.tx_extract_idx = 0; + + // Initialize the binding pin + gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN); + + // Initialize the IRQ/DRDY pin + gpio_setup_input(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN); + + // Initialize the cyrf6936 chip + cyrf6936_init(&superbitrf.cyrf6936, &(SUPERBITRF_SPI_DEV), 2, SUPERBITRF_RST_PORT, SUPERBITRF_RST_PIN); +} + +/** + * The superbitrf on event call + */ +void superbitrf_event(void) { + uint8_t i, pn_row, data_code[16]; + static uint8_t packet_size, tx_packet[16]; + static bool_t start_transfer = TRUE; + + // Check if the cyrf6936 isn't busy and the uperbitrf is initialized + if(superbitrf.cyrf6936.status != CYRF6936_IDLE) + return; + + // When the device is initialized handle the IRQ + if(superbitrf.status != SUPERBITRF_UNINIT) { + // First handle the IRQ + if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { + // Receive the packet + cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936); + superbitrf.irq_count++; + } + + /* Check if it is a valid receive */ + if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { + // Handle the packet received + superbitrf_receive_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_RXE_IRQ), superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet); + superbitrf.rx_packet_count++; + + // Reset the packet receiving + superbitrf.cyrf6936.has_irq = FALSE; + } + + /* Check if it has a valid send */ + if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.tx_irq_status & CYRF_TXC_IRQ)) { + // Handle the send packet + superbitrf_send_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_TXE_IRQ)); + superbitrf.tx_packet_count++; + + // Reset the packet receiving + superbitrf.cyrf6936.has_irq = FALSE; + } + } + + // Check the status of the superbitrf + switch(superbitrf.status) { + + /* When the superbitrf isn't initialized */ + case SUPERBITRF_UNINIT: + // Try to write the startup config + if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_stratup_config, 11)) { + // Check if need to go to bind or transfer + if(gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) + start_transfer = FALSE; + + superbitrf.status = SUPERBITRF_INIT_BINDING; + } + break; + + /* When the superbitrf is initializing binding */ + case SUPERBITRF_INIT_BINDING: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // Try to write the binding config + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5); + superbitrf.state++; + break; + case 1: + // Set the data code and channel + memcpy(data_code, pn_codes[0][8], 8); + memcpy(data_code + 8, pn_bind, 8); + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, 1, pn_codes[0][0], data_code, 0x0000); + + superbitrf.status = SUPERBITRF_BINDING; + break; + default: + // Should not happen + superbitrf.state = 0; + break; + } + break; + + /* When the superbitrf is initializing transfer */ + case SUPERBITRF_INIT_TRANSFER: + // Generate the DSMX channels + superbitrf_gen_dsmx_channels(); + + // Try to write the transfer config + if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4)) { + superbitrf.resync_count = 0; + superbitrf.packet_loss = FALSE; + superbitrf.packet_loss_bit = 0; + superbitrf.status = SUPERBITRF_SYNCING_A; + superbitrf.state = 1; + } + break; + + /* When the superbitrf is in binding mode */ + case SUPERBITRF_BINDING: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.state++; + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + + superbitrf.state++; + break; + case 2: + // Switch channel + superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define + cyrf6936_write(&superbitrf.cyrf6936, CYRF_CHANNEL, superbitrf.channel); + + superbitrf.state += 2; // Already aborted + break; + case 3: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + + superbitrf.state++; + break; + case 4: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Check if need to go to transfer + if(start_transfer) { + // Initialize the binding values + #ifdef RADIO_TRANSMITTER_ID + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_ID); + superbitrf.bind_mfg_id32 = RADIO_TRANSMITTER_ID; + superbitrf.bind_mfg_id[0] = (superbitrf.bind_mfg_id32 &0xFF); + superbitrf.bind_mfg_id[1] = (superbitrf.bind_mfg_id32 >>8 &0xFF); + superbitrf.bind_mfg_id[2] = (superbitrf.bind_mfg_id32 >>16 &0xFF); + superbitrf.bind_mfg_id[3] = (superbitrf.bind_mfg_id32 >>24 &0xFF); + + // Calculate some values based on the bind MFG id + superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]); + superbitrf.sop_col = (superbitrf.bind_mfg_id[0] + superbitrf.bind_mfg_id[1] + superbitrf.bind_mfg_id[2] + 2) & 0x07; + superbitrf.data_col = 7 - superbitrf.sop_col; + #endif + #ifdef RADIO_TRANSMITTER_CHAN + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_CHAN); + superbitrf.num_channels = RADIO_TRANSMITTER_CHAN; + #endif + #ifdef RADIO_TRANSMITTER_PROTOCOL + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_PROTOCOL); + superbitrf.protocol = RADIO_TRANSMITTER_PROTOCOL; + superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; + #endif + + // Start transfer + superbitrf.state = 0; + superbitrf.status = SUPERBITRF_INIT_TRANSFER; + break; + } + + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_BIND_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } + break; + + /* When the receiver is synchronizing with the transmitter */ + case SUPERBITRF_SYNCING_A: + case SUPERBITRF_SYNCING_B: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.state++; + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 2: + // Switch channel, sop code, data code and crc + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23; + pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channels[superbitrf.channel_idx] % 5 : (superbitrf.channels[superbitrf.channel_idx]-2) % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channels[superbitrf.channel_idx], + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + superbitrf.state++; + break; + case 3: + // Create a new packet when no packet loss + if(!superbitrf.packet_loss) { + superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = (superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; + } + + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); + if(packet_size > 14) + packet_size = 14; + + for(i = 0; i < packet_size; i++) + tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + } + + // Send a packet + cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); + + // Update the packet extraction + if(!superbitrf.packet_loss) + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + + superbitrf.state++; + break; + case 4: + //TODO: check timeout? (Waiting for send) + break; + case 5: + superbitrf.state = 7; + break; + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECVB_TIME) % 0xFFFFFFFF; + superbitrf.state++; + break; + case 6: + // Wait for telemetry data + if (superbitrf.timer < get_sys_time_usec()) + superbitrf.state++; + break; + case 7: + // When DSMX we don't need to switch + if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { + superbitrf.state++; + superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + break; + } + + // Switch channel, sop code, data code and crc + superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define + superbitrf.crc_seed = ~superbitrf.crc_seed; + pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + + superbitrf.state++; + break; + case 8: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_SYNC_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } + break; + + /* Normal transfer mode */ + case SUPERBITRF_TRANSFER: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // Fixing timer overflow + if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) + superbitrf.timer_overflow = FALSE; + + // When there is a timeout + if(superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) { + superbitrf.transfer_timeouts++; + superbitrf.timeouts++; + superbitrf.state++; + } + + // We really lost the communication + if(superbitrf.timeouts > 100) { + superbitrf.state = 0; + superbitrf.resync_count++; + superbitrf.status = SUPERBITRF_SYNCING_A; + } + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; + if(superbitrf.timer < get_sys_time_usec()) + superbitrf.timer_overflow = TRUE; + else + superbitrf.timer_overflow = FALSE; + + // Only send on channel 2 + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) + superbitrf.state = 8; + break; + case 2: + // Wait before sending (FIXME??) + superbitrf.state++; + break; + case 3: + // Create a new packet when no packet loss + if(!superbitrf.packet_loss) { + superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = ((~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = ((superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF; + } + + packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128); + if(packet_size > 14) + packet_size = 14; + + for(i = 0; i < packet_size; i++) + tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128]; + } + + // Send a packet + cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); + + // Update the packet extraction + if(!superbitrf.packet_loss) + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128; + + superbitrf.state++; + break; + case 4: + //TODO: check timeout? (Waiting for send) + break; + case 5: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + case 6: + // Fixing timer overflow + if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) + superbitrf.timer_overflow = FALSE; + + // Waiting for data receive + if (superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) + superbitrf.state++; + break; + case 7: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 8: + // Switch channel, sop code, data code and crc + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23; + superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + superbitrf.crc_seed = ~superbitrf.crc_seed; + pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + + superbitrf.state++; + break; + case 9: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; + else + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_SHORT_TIME) % 0xFFFFFFFF; + if(superbitrf.timer < get_sys_time_usec()) + superbitrf.timer_overflow = TRUE; + else + superbitrf.timer_overflow = FALSE; + + superbitrf.state = 0; + break; + } + break; + + /* Should not come here */ + default: + break; + } +} + +/** + * When we receive a packet this callback is called + */ +static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]) { + int i; + uint16_t sum; + + /* Switch on the status of the superbitRF */ + switch (superbitrf.status) { + + /* When we are in binding mode */ + case SUPERBITRF_BINDING: + // Check if the MFG id is exactly the same + if (packet[0] != packet[4] || packet[1] != packet[5] || packet[2] != packet[6] || packet[3] != packet[7]) { + // Start receiving without changing channel + superbitrf.state = 3; + break; + } + + // Calculate the first sum + sum = 384 - 0x10; + for (i = 0; i < 8; i++) + sum += packet[i]; + + // Check the first sum + if (packet[8] != sum >> 8 || packet[9] != (sum & 0xFF)) { + // Start receiving without changing channel + superbitrf.state = 3; + break; + } + + // Calculate second sum + for (i = 8; i < 14; i++) + sum += packet[i]; + + // Check the second sum + if (packet[14] != sum >> 8 || packet[15] != (sum & 0xFF)) { + // Start receiving without changing channel + superbitrf.state = 3; + break; + } + + // Update the mfg id, number of channels and protocol + superbitrf.bind_mfg_id[0] = ~packet[0]; + superbitrf.bind_mfg_id[1] = ~packet[1]; + superbitrf.bind_mfg_id[2] = ~packet[2]; + superbitrf.bind_mfg_id[3] = ~packet[3]; + superbitrf.bind_mfg_id32 = ((superbitrf.bind_mfg_id[3] &0xFF) << 24 | (superbitrf.bind_mfg_id[2] &0xFF) << 16 | + (superbitrf.bind_mfg_id[1] &0xFF) << 8 | (superbitrf.bind_mfg_id[0] &0xFF)); + superbitrf.num_channels = packet[11]; + superbitrf.protocol = packet[12]; + superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; + + // Calculate some values based on the bind MFG id + superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]); + superbitrf.sop_col = (superbitrf.bind_mfg_id[0] + superbitrf.bind_mfg_id[1] + superbitrf.bind_mfg_id[2] + 2) & 0x07; + superbitrf.data_col = 7 - superbitrf.sop_col; + + // Update the status of the receiver + superbitrf.state = 0; + superbitrf.status = SUPERBITRF_INIT_TRANSFER; + break; + + /* When we receive a packet during syncing first channel A */ + case SUPERBITRF_SYNCING_A: + // Check the MFG id + if(error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) + superbitrf.crc_seed = ~superbitrf.crc_seed; + + // When we receive a data packet + if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { + superbitrf.uplink_count++; + + // Check if it is a data loss packet + if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF) + superbitrf.packet_loss = TRUE; + else + superbitrf.packet_loss = FALSE; + + // When it is a data packet, parse the packet if not busy already + if(!dl_msg_available && !superbitrf.packet_loss) { + for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans.msg_received = FALSE; + } + } + } + break; + } + + if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channels[1] = superbitrf.channel; + + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_SYNCING_B; + } else { + superbitrf.timeouts = 0; + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_TRANSFER; + } + break; + + /* When we receive a packet during syncing second channel B */ + case SUPERBITRF_SYNCING_B: + // Check the MFG id + if(error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) + superbitrf.crc_seed = ~superbitrf.crc_seed; + + // When we receive a data packet + if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { + superbitrf.uplink_count++; + + // When it is a data packet, parse the packet if not busy already + if(!dl_msg_available) { + for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans.msg_received = FALSE; + } + } + } + break; + } + + // Set the channel + if(superbitrf.channels[0] != superbitrf.channel) { + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channel_idx = 0; + } + else { + superbitrf.channels[1] = superbitrf.channel; + superbitrf.channel_idx = 1; + } + + // When the channels aren't the same go to transfer mode + if(superbitrf.channels[1] != superbitrf.channels[0]) { + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_TRANSFER; + superbitrf.timeouts = 0; + } + break; + + /* When we receive a packet during transfer */ + case SUPERBITRF_TRANSFER: + // Check the MFG id + if(error) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + + // Check if it is a RC packet + if(packet[1] == (~superbitrf.bind_mfg_id[3]&0xFF) || packet[1] == (superbitrf.bind_mfg_id[3]&0xFF)) { + superbitrf.rc_count++; + + // Parse the packet + superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); + superbitrf.rc_frame_available = TRUE; + + // Calculate the timing (seperately for the channel switches) + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) + superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); + else + superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_SHORT_TIME); + + // Go to next receive + superbitrf.state = 1; + superbitrf.timeouts = 0; + } else { + superbitrf.uplink_count++; + + // Check if it is a data loss packet + if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)) + superbitrf.packet_loss = TRUE; + else + superbitrf.packet_loss = FALSE; + + superbitrf.packet_loss = FALSE; + + // When it is a data packet, parse the packet if not busy already + if(!dl_msg_available && !superbitrf.packet_loss) { + for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans.msg_received = FALSE; + } + } + } + + // Update the state + superbitrf.state = 7; + } + break; + + /* Should not come here */ + default: + break; + } +} + +static inline void superbitrf_send_packet_cb(bool_t error) { + /* Switch on the status of the superbitRF */ + switch (superbitrf.status) { + + /* When we are synchronizing */ + case SUPERBITRF_SYNCING_A: + case SUPERBITRF_SYNCING_B: + // When we successfully or unsuccessfully send a data packet + if(superbitrf.state == 4) + superbitrf.state++; + break; + + /* When we are in transfer mode */ + case SUPERBITRF_TRANSFER: + // When we successfully or unsuccessfully send a packet + if(superbitrf.state == 4) + superbitrf.state++; + break; + + /* Should not come here */ + default: + break; + } +} + +/** + * Parse a radio channel packet + */ +static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels) { + int i; + uint8_t bit_shift = (is_11bit)? 11:10; + int16_t value_max = (is_11bit)? 0x07FF: 0x03FF; + + for (i=0; i<7; i++) { + const int16_t tmp = ((data[2*i]<<8) + data[2*i+1]) & 0x7FFF; + const uint8_t chan = (tmp >> bit_shift) & 0x0F; + const int16_t val = (tmp&value_max); + + if(chan < nb_channels) { + channels[chan] = val; + + // Scale the channel + if(is_11bit) { + channels[chan] -= 0x400; + channels[chan] *= MAX_PPRZ/0x2AC; + } else { + channels[chan] -= 0x200; + channels[chan] *= MAX_PPRZ/0x156; + } + } + } +} + +/** + * Generate the channels + */ +static inline void superbitrf_gen_dsmx_channels(void) { + // Calculate the DSMX channels + int idx = 0; + uint32_t id = ~((superbitrf.bind_mfg_id[0] << 24) | (superbitrf.bind_mfg_id[1] << 16) | + (superbitrf.bind_mfg_id[2] << 8) | (superbitrf.bind_mfg_id[3] << 0)); + uint32_t id_tmp = id; + + // While not all channels are set + while(idx < 23) { + int i; + int count_3_27 = 0, count_28_51 = 0, count_52_76 = 0; + + id_tmp = id_tmp * 0x0019660D + 0x3C6EF35F; // Randomization + uint8_t next_ch = ((id_tmp >> 8) % 0x49) + 3; // Use least-significant byte and must be larger than 3 + if (((next_ch ^ id) & 0x01 ) == 0) + continue; + + // Go trough all already set channels + for (i = 0; i < idx; i++) { + // Channel is already used + if(superbitrf.channels[i] == next_ch) + break; + + // Count the channel groups + if(superbitrf.channels[i] <= 27) + count_3_27++; + else if (superbitrf.channels[i] <= 51) + count_28_51++; + else + count_52_76++; + } + + // When channel is already used continue + if (i != idx) + continue; + + // Set the channel when channel groups aren't full + if ((next_ch < 28 && count_3_27 < 8) // Channels 3-27: max 8 + || (next_ch >= 28 && next_ch < 52 && count_28_51 < 7) // Channels 28-52: max 7 + || (next_ch >= 52 && count_52_76 < 8)) { // Channels 52-76: max 8 + superbitrf.channels[idx++] = next_ch; + } + } +} + + + diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h new file mode 100644 index 0000000000..e1cbb6d3bf --- /dev/null +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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/datalink/superbitrf.h + * DSM2 and DSMX datalink implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#ifndef DATALINK_SUPERBITRF_H +#define DATALINK_SUPERBITRF_H + +#include "mcu_periph/gpio.h" +#include "peripherals/cyrf6936.h" +#include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/pprz_transport.h" + +/* The timings in microseconds */ +#define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ +#define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ +#define SUPERBITRF_RECV_TIME 20000 /**< The time to wait for a transfer packet on a channel in microseconds */ +#define SUPERBITRF_RECV_SHORT_TIME 6000 /**< The time to wait for a transfer packet short on a channel in microseconds */ +#define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */ +#define SUPERBITRF_DATARECVB_TIME 6000 /**< The time to wait for a data packet on a channel during bind in microseconds */ + +/* The different statuses the superbitRF can be in */ +enum SuperbitRFStatus { + SUPERBITRF_UNINIT, /**< The chip isn't initialized */ + SUPERBITRF_INIT_BINDING, /**< The chip is initializing binding mode */ + SUPERBITRF_INIT_TRANSFER, /**< The chip is initializing transfer mode */ + SUPERBITRF_BINDING, /**< The chip is in binding mode */ + SUPERBITRF_SYNCING_A, /**< The chip is in synchronizing mode for channel A */ + SUPERBITRF_SYNCING_B, /**< The chip is in synchronizing mode for channel B */ + SUPERBITRF_TRANSFER, /**< The chip is in transfer mode */ +}; + +/* The different resolutions a transmitter can be in */ +enum dsm_resolution { + SUPERBITRF_10_BIT_RESOLUTION = 0x00, /**< The transmitter has a 10 bit resolution */ + SUPERBITRF_11_BIT_RESOLUTION = 0x01, /**< The transmitter has a 11 bit resolution */ +}; + +/* The different protocols a transmitter can send */ +enum dsm_protocol { + DSM_DSM2_1 = 0x01, /**< The original DSM2 protocol with 1 packet of data */ + DSM_DSM2_2 = 0x02, /**< The original DSM2 protocol with 2 packets of data */ + DSM_DSM2P = 0x10, /**< Our own DSM2 Paparazzi protocol */ + DSM_DSMXP = 0x11, /**< Our own DSMX Paparazzi protocol */ + DSM_DSMX_1 = 0xA2, /**< The original DSMX protocol with 1 packet of data */ + DSM_DSMX_2 = 0xB2, /**< The original DSMX protocol with 2 packets of data */ +}; +#define IS_DSM2(x) (x == DSM_DSM2P || x == DSM_DSM2_1 || x == DSM_DSM2_2) +#define IS_DSMX(x) (!IS_DSM2(x)) + +/* The superbitrf structure */ +struct SuperbitRF { + struct Cyrf6936 cyrf6936; /**< The cyrf chip used */ + volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */ + uint8_t state; /**< The states each status can be in */ + uint32_t timer; /**< The timer in microseconds */ + bool_t timer_overflow; /**< When the timer overflows */ + uint8_t timeouts; /**< The amount of timeouts */ + uint32_t transfer_timeouts; /**< The amount of timeouts during transfer */ + uint32_t resync_count; /**< The amount of resyncs needed during transfer */ + uint8_t packet_loss_bit; /**< The packet loss indicating bit */ + bool_t packet_loss; /**< When we have packet loss last packet */ + + uint8_t channels[23]; /**< The channels used for DSM2/DSMX */ + uint8_t channel_idx; /**< The current channel index */ + uint8_t channel; /**< The current channel number */ + uint32_t irq_count; /**< How many interrupts are made */ + uint32_t rx_packet_count; /**< How many packets are received(also the invalid) */ + uint32_t tx_packet_count; /**< How many packets are send(also the invalid) */ + uint32_t uplink_count; /**< How many valid uplink packages are received */ + uint32_t rc_count; /**< How many valid RC packages are received */ + + uint8_t bind_mfg_id[4]; /**< The MFG id where the receiver is bound to */ + uint32_t bind_mfg_id32; /**< The MFG id where the receiver is bound to in uint32 */ + uint8_t num_channels; /**< The number of channels the transmitter has */ + volatile enum dsm_protocol protocol; /**< The protocol the transmitter uses */ + volatile enum dsm_resolution resolution; /**< The resolution that the transmitter has */ + uint16_t crc_seed; /**< The CRC seed that is calculated with the bind MFG id */ + uint8_t sop_col; /**< The sop code column number calculated with the bind MFG id */ + uint8_t data_col; /**< The data code column number calculated with the bind MFG id */ + + bool_t rc_frame_available; /**< When a RC frame is available */ + uint32_t timing1; /**< Time between last receive in microseconds */ + uint32_t timing2; /**< Time between second last receive in microseconds */ + int16_t rc_values[14]; /**< The rc values from the packet */ + + struct pprz_transport rx_transport; /**< The receive transport */ + + uint8_t tx_buffer[128]; /**< The transmit buffer */ + uint8_t tx_insert_idx; /**< The transmit buffer insert index */ + uint8_t tx_extract_idx; /**< The transmit buffer extract index */ +}; + +/* The superbitrf functions and structures */ +extern struct SuperbitRF superbitrf; +void superbitrf_init(void); +void superbitrf_event(void); + +/* The datalink defines */ +#define SuperbitRFInit() { }//superbitrf_init(); } +#define SuperbitRFCheckFreeSpace(_x) (((superbitrf.tx_insert_idx+1) %128) != superbitrf.tx_extract_idx) +#define SuperbitRFTransmit(_x) { \ + superbitrf.tx_buffer[superbitrf.tx_insert_idx] = _x; \ + superbitrf.tx_insert_idx = (superbitrf.tx_insert_idx+1) %128; \ + } +#define SuperbitRFSendMessage() { } +#define SuperbitRFCheckAndParse() { } + +#endif /* DATALINK_SUPERBITRF_H */ diff --git a/sw/airborne/subsystems/datalink/udp.c b/sw/airborne/subsystems/datalink/udp.c index 9d9a717490..88ab870c1d 100644 --- a/sw/airborne/subsystems/datalink/udp.c +++ b/sw/airborne/subsystems/datalink/udp.c @@ -26,7 +26,7 @@ //Check if variables are set and else define them #ifndef LINK_HOST -#define LINK_HOST "192.168.1.0" +#define LINK_HOST "192.168.1.255" #endif #ifndef LINK_PORT #define LINK_PORT 4242 diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index d0075cdd0c..e7103e8075 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -1,3 +1,30 @@ +/* + * 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. + */ + +/** + * @file subsystems/electrical.c + * + * Implemnetation for electrical status: supply voltage, current, battery status, etc. + */ + #include "subsystems/electrical.h" #include "mcu_periph/adc.h" @@ -19,6 +46,15 @@ #define COMMAND_CURRENT_ESTIMATION COMMAND_THRUST #endif +#ifndef BAT_CHECKER_DELAY +#define BAT_CHECKER_DELAY 5 +#endif + +#define ELECTRICAL_PERIODIC_FREQ 10 + +PRINT_CONFIG_VAR(LOW_BAT_LEVEL) +PRINT_CONFIG_VAR(CRITIC_BAT_LEVEL) + struct Electrical electrical; #if defined ADC_CHANNEL_VSUPPLY || defined ADC_CHANNEL_CURRENT || defined MILLIAMP_AT_FULL_THROTTLE @@ -50,6 +86,9 @@ void electrical_init(void) { electrical.vsupply = 0; electrical.current = 0; + electrical.bat_low = FALSE; + electrical.bat_critical = FALSE; + #if defined ADC_CHANNEL_VSUPPLY adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE); #endif @@ -64,6 +103,9 @@ PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY) } void electrical_periodic(void) { + static uint8_t bat_low_counter = 0; + static uint8_t bat_critical_counter = 0; + #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample)); #endif @@ -93,4 +135,29 @@ void electrical_periodic(void) { electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); #endif /* ADC_CHANNEL_CURRENT */ + + if (electrical.vsupply < LOW_BAT_LEVEL * 10) { + if (bat_low_counter > 0) + bat_low_counter--; + if (bat_low_counter == 0) + electrical.bat_low = TRUE; + } + else { + // reset battery low status and counter + bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ; + electrical.bat_low = FALSE; + } + + if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) { + if (bat_critical_counter > 0) + bat_critical_counter--; + if (bat_critical_counter == 0) + electrical.bat_critical = TRUE; + } + else { + // reset battery critical status and counter + bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ; + electrical.bat_critical = FALSE; + } + } diff --git a/sw/airborne/subsystems/electrical.h b/sw/airborne/subsystems/electrical.h index d9b70f9a2f..81d70fbda2 100644 --- a/sw/airborne/subsystems/electrical.h +++ b/sw/airborne/subsystems/electrical.h @@ -1,13 +1,55 @@ +/* + * 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. + */ + +/** + * @file subsystems/electrical.h + * + * Interface for electrical status: supply voltage, current, battery status, etc. + */ + #ifndef SUBSYSTEMS_ELECTRICAL_H #define SUBSYSTEMS_ELECTRICAL_H #include "std.h" +#include "generated/airframe.h" + +/** low battery level in Volts (for 3S LiPo) */ +#ifndef LOW_BAT_LEVEL +#define LOW_BAT_LEVEL 10.5 +#endif + + +/** critical battery level in Volts (for 3S LiPo) */ +#ifndef CRITIC_BAT_LEVEL +#define CRITIC_BAT_LEVEL 9.8 +#endif + struct Electrical { - uint16_t vsupply; ///< supply voltage in decivolts - int32_t current; ///< current in milliamps - int32_t consumed; ///< consumption in mAh + uint16_t vsupply; ///< supply voltage in decivolts + int32_t current; ///< current in milliamps + int32_t consumed; ///< consumption in mAh + bool_t bat_low; ///< battery low status + bool_t bat_critical; ///< battery critical status }; diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c index 0326f0dd64..2feba4c765 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.c +++ b/sw/airborne/subsystems/gps/gps_ardrone2.c @@ -26,6 +26,10 @@ * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2. */ +#ifdef ARDRONE2_DEBUG +# include +#endif + #include "subsystems/gps.h" #include "math/pprz_geodetic_double.h" @@ -37,6 +41,10 @@ void gps_impl_init( void ) { void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { int i; + +#ifdef ARDRONE2_DEBUG + printf("state = %d\n", navdata_gps->gps_state); +#endif // Set the lla double struct from the navdata struct LlaCoor_d gps_lla_d; gps_lla_d.lat = RadOfDeg(navdata_gps->lat); diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 55c935af60..546d32d549 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -22,7 +22,14 @@ #include "subsystems/gps/gps_sim_nps.h" #include "subsystems/gps.h" +#if GPS_USE_LATLONG +/* currently needed to get nav_utm_zone0 */ +#include "subsystems/navigation/common_nav.h" +#include "math/pprz_geodetic_float.h" +#endif + bool_t gps_available; +bool_t gps_has_fix; void gps_feed_value() { gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; @@ -36,10 +43,47 @@ void gps_feed_value() { gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7; gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; gps.hmsl = sensors.gps.hmsl * 1000.; - gps.fix = GPS_FIX_3D; + + /* calc NED speed from ECEF */ + struct LtpDef_d ref_ltp; + ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos); + struct NedCoor_d ned_vel_d; + ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel); + gps.ned_vel.x = ned_vel_d.x * 100; + gps.ned_vel.y = ned_vel_d.y * 100; + gps.ned_vel.z = ned_vel_d.z * 100; + + /* horizontal and 3d ground speed in cm/s */ + gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100; + gps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100; + + /* ground course in radians * 1e7 */ + gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7; + +#if GPS_USE_LATLONG + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla_f; + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; + struct UtmCoor_f utm_f; + utm_f.zone = nav_utm_zone0; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = gps.lla_pos.alt; + gps.utm_pos.zone = nav_utm_zone0; +#endif + + if (gps_has_fix) + gps.fix = GPS_FIX_3D; + else + gps.fix = GPS_FIX_NONE; gps_available = TRUE; } void gps_impl_init() { gps_available = FALSE; + gps_has_fix = TRUE; } diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h index e1dd5fc05d..9c327590a2 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.h +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -6,6 +6,7 @@ #define GPS_NB_CHANNELS 16 extern bool_t gps_available; +extern bool_t gps_has_fix; extern void gps_feed_value(); diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h index 18b1b1bbad..cbc7c33b19 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h @@ -116,9 +116,9 @@ static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _a //checks if the navboard has a new dataset ready if (navdata_imu_available == TRUE) { navdata_imu_available = FALSE; - RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, -navdata->vy, -navdata->vz); - VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, 4096-navdata->ay, 4096-navdata->az); - VECT3_ASSIGN(imu.mag_unscaled, -navdata->mx, -navdata->my, -navdata->mz); + RATES_ASSIGN(imu.gyro_unscaled, navdata.vx, -navdata.vy, -navdata.vz); + VECT3_ASSIGN(imu.accel_unscaled, navdata.ax, 4096-navdata.ay, 4096-navdata.az); + VECT3_ASSIGN(imu.mag_unscaled, -navdata.mx, -navdata.my, -navdata.mz); _gyro_handler(); _accel_handler(); diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index 0fcd038a6e..ef708b53ee 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -174,10 +174,28 @@ void imu_aspirin2_event(void) -imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); +#else +#ifdef LISA_S +#ifdef LISA_S_UPSIDE_DOWN + RATES_ASSIGN(imu.gyro_unscaled, + imu_aspirin2.mpu.data_rates.rates.p, + -imu_aspirin2.mpu.data_rates.rates.q, + -imu_aspirin2.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + imu_aspirin2.mpu.data_accel.vect.x, + -imu_aspirin2.mpu.data_accel.vect.y, + -imu_aspirin2.mpu.data_accel.vect.z); + VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z); +#else + RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); + VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); + VECT3_COPY(imu.mag_unscaled, mag); +#endif #else RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z) +#endif #endif imu_aspirin2.mpu.data_available = FALSE; imu_aspirin2.gyro_valid = TRUE; diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.c b/sw/airborne/subsystems/imu/imu_px4fmu.c new file mode 100644 index 0000000000..73be88f475 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4fmu.c @@ -0,0 +1,122 @@ +/* + * 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/imu/imu_px4fmu.c + * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883. + */ + +#include "subsystems/imu.h" + +#include "mcu_periph/spi.h" +#include "peripherals/hmc58xx_regs.h" + + +/* MPU60x0 gyro/accel internal lowpass frequency */ +#if !defined PX4FMU_LOWPASS_FILTER && !defined PX4FMU_SMPLRT_DIV +#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) +/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms + * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz + */ +#define PX4FMU_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define PX4FMU_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") +#elif PERIODIC_FREQUENCY == 512 +/* Accelerometer: Bandwidth 260Hz, Delay 0ms + * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz + */ +#define PX4FMU_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define PX4FMU_SMPLRT_DIV 3 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") +#else +#error Non-default PERIODIC_FREQUENCY: please define PX4FMU_LOWPASS_FILTER and PX4FMU_SMPLRT_DIV. +#endif +#endif +PRINT_CONFIG_VAR(PX4FMU_LOWPASS_FILTER) +PRINT_CONFIG_VAR(PX4FMU_SMPLRT_DIV) + +#ifndef PX4FMU_GYRO_RANGE +#define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 +#endif +PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE) + +#ifndef PX4FMU_ACCEL_RANGE +#define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G +#endif +PRINT_CONFIG_VAR(PX4FMU_ACCEL_RANGE) + +struct ImuPx4fmu imu_px4fmu; + +void imu_impl_init(void) +{ + imu_px4fmu.accel_valid = FALSE; + imu_px4fmu.gyro_valid = FALSE; + imu_px4fmu.mag_valid = FALSE; + + /* MPU is on spi1 and CS is SLAVE2 */ + mpu60x0_spi_init(&imu_px4fmu.mpu, &spi1, SPI_SLAVE2); + // change the default configuration + imu_px4fmu.mpu.config.smplrt_div = PX4FMU_SMPLRT_DIV; + imu_px4fmu.mpu.config.dlpf_cfg = PX4FMU_LOWPASS_FILTER; + imu_px4fmu.mpu.config.gyro_range = PX4FMU_GYRO_RANGE; + imu_px4fmu.mpu.config.accel_range = PX4FMU_ACCEL_RANGE; + + /* initialize mag on i2c2 and set default options */ + hmc58xx_init(&imu_px4fmu.hmc, &i2c2, HMC58XX_ADDR); +} + + +void imu_periodic(void) +{ + mpu60x0_spi_periodic(&imu_px4fmu.mpu); + + // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz) + RunOnceEvery(10, hmc58xx_periodic(&imu_px4fmu.hmc)); +} + +void imu_px4fmu_event(void) +{ + mpu60x0_spi_event(&imu_px4fmu.mpu); + if (imu_px4fmu.mpu.data_available) { + RATES_ASSIGN(imu.gyro_unscaled, + imu_px4fmu.mpu.data_rates.rates.q, + imu_px4fmu.mpu.data_rates.rates.p, + -imu_px4fmu.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + imu_px4fmu.mpu.data_accel.vect.y, + imu_px4fmu.mpu.data_accel.vect.x, + -imu_px4fmu.mpu.data_accel.vect.z); + imu_px4fmu.mpu.data_available = FALSE; + imu_px4fmu.gyro_valid = TRUE; + imu_px4fmu.accel_valid = TRUE; + } + + /* HMC58XX event task */ + hmc58xx_event(&imu_px4fmu.hmc); + if (imu_px4fmu.hmc.data_available) { + imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y; + imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x; + imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z; + imu_px4fmu.hmc.data_available = FALSE; + imu_px4fmu.mag_valid = TRUE; + } +} + diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.h b/sw/airborne/subsystems/imu/imu_px4fmu.h new file mode 100644 index 0000000000..1620b9892c --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_px4fmu.h @@ -0,0 +1,83 @@ +/* + * 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/imu/imu_px4fmu.h + * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883. + */ + +#ifndef IMU_PX4FMU_SPI_H +#define IMU_PX4FMU_SPI_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "subsystems/imu/imu_mpu60x0_defaults.h" +#include "peripherals/mpu60x0_spi.h" +#include "peripherals/hmc58xx.h" + +#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN +#define IMU_GYRO_P_SIGN 1 +#define IMU_GYRO_Q_SIGN 1 +#define IMU_GYRO_R_SIGN 1 +#endif +#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN +#define IMU_ACCEL_X_SIGN 1 +#define IMU_ACCEL_Y_SIGN 1 +#define IMU_ACCEL_Z_SIGN 1 +#endif +#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN +#define IMU_MAG_X_SIGN 1 +#define IMU_MAG_Y_SIGN 1 +#define IMU_MAG_Z_SIGN 1 +#endif + +struct ImuPx4fmu { + volatile bool_t gyro_valid; + volatile bool_t accel_valid; + volatile bool_t mag_valid; + struct Mpu60x0_Spi mpu; + struct Hmc58xx hmc; +}; + +extern struct ImuPx4fmu imu_px4fmu; + +extern void imu_px4fmu_event(void); + + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { + imu_px4fmu_event(); + if (imu_px4fmu.gyro_valid) { + imu_px4fmu.gyro_valid = FALSE; + _gyro_handler(); + } + if (imu_px4fmu.accel_valid) { + imu_px4fmu.accel_valid = FALSE; + _accel_handler(); + } + if (imu_px4fmu.mag_valid) { + imu_px4fmu.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif /* IMU_PX4FMU_H */ diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.c b/sw/airborne/subsystems/radio_control/superbitrf_rc.c new file mode 100644 index 0000000000..f3609ed3b8 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.c @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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/radio_control/superbitrf_rc.c + * DSM2 and DSMX radio control implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#include "superbitrf_rc.h" +#include "subsystems/radio_control.h" + +/** + * Initialization + */ +//#if DATALINK == SUPERBITRF +//void radio_control_impl_init(void) {} +//#else +void radio_control_impl_init(void) { superbitrf_init(); } +//#endif diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.h b/sw/airborne/subsystems/radio_control/superbitrf_rc.h new file mode 100644 index 0000000000..de410a56d3 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.h @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2013 Freek van Tienen + * + * 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/radio_control/superbitrf_rc.h + * DSM2 and DSMX radio control implementation for the cyrf6936 2.4GHz radio chip trough SPI + */ + +#ifndef RADIO_CONTROL_SUPERBITRF_RC_H +#define RADIO_CONTROL_SUPERBITRF_RC_H + +#include "subsystems/datalink/superbitrf.h" + +/* Theoretically you could have 14 channel over DSM2/DSMX */ +#ifndef RADIO_CONTROL_NB_CHANNEL +#define RADIO_CONTROL_NB_CHANNEL 14 +#endif + +/* The channel ordering is always the same for DSM2 and DSMX */ +#define RADIO_THROTTLE 0 +#define RADIO_ROLL 1 +#define RADIO_PITCH 2 +#define RADIO_YAW 3 +#define RADIO_GEAR 4 +#define RADIO_FLAP 5 +#define RADIO_AUX1 5 +#define RADIO_AUX2 6 +#define RADIO_AUX3 7 +#define RADIO_AUX4 8 +#define RADIO_AUX5 9 +#define RADIO_AUX6 10 +#define RADIO_AUX7 11 +#define RADIO_AUX8 12 +#define RADIO_AUX9 13 + +/* Map the MODE default to the gear switch */ +#ifndef RADIO_MODE +#define RADIO_MODE RADIO_GEAR +#endif + +/* Macro that normalize superbitrf rc_values to radio values */ +#define NormalizeRcDl(_in, _out, _count) { \ + uint8_t i; \ + for(i = 0; i < _count; i++) { \ + if(i == RADIO_THROTTLE) { \ + _out[i] = (_in[i] + MAX_PPRZ) / 2; \ + Bound(_out[i], 0, MAX_PPRZ); \ + } else { \ + _out[i] = -_in[i]; \ + Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \ + } \ + } \ +} + +/* The radio control event handler */ +#define RadioControlEvent(_received_frame_handler) { \ + cyrf6936_event(&superbitrf.cyrf6936); \ + superbitrf_event(); \ + if(superbitrf.rc_frame_available) { \ + radio_control.frame_cpt++; \ + radio_control.time_since_last_frame = 0; \ + radio_control.radio_ok_cpt = 0; \ + radio_control.status = RC_OK; \ + NormalizeRcDl(superbitrf.rc_values,radio_control.values \ + ,superbitrf.num_channels); \ + _received_frame_handler(); \ + superbitrf.rc_frame_available = FALSE; \ + } \ +} + +#endif /* RADIO_CONTROL_SUPERBITRF_RC_H */ diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c index a996cee2dc..a3c446e4bd 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c @@ -21,7 +21,7 @@ */ /** - * @file boards/lisa_m/baro_ms5611_i2c.c + * @file subsystems/sensors/baro_ms5611_i2c.c * * Driver for MS5611 baro via I2C. * diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c index a86ab818f7..bd8603ea43 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c +++ b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c @@ -21,7 +21,7 @@ */ /** - * @file boards/lisa_m/baro_ms5611_spi.c + * @file subsystems/sensors/baro_ms5611_spi.c * * Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI. * diff --git a/sw/airborne/subsystems/sonar.h b/sw/airborne/subsystems/sonar.h new file mode 100644 index 0000000000..daf310400d --- /dev/null +++ b/sw/airborne/subsystems/sonar.h @@ -0,0 +1,4 @@ + + + +extern uint16_t sonar_meas; 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/ext/ardrone2_drivers/check_update.sh b/sw/ext/ardrone2_drivers/check_update.sh new file mode 100755 index 0000000000..3fee525f48 --- /dev/null +++ b/sw/ext/ardrone2_drivers/check_update.sh @@ -0,0 +1,96 @@ +#!/bin/sh + +UPDATE_DIR=/update +UPDATE_PATH=$UPDATE_DIR/ardrone2_update.plf +VERSION_PATH=$UPDATE_DIR/version.txt +ERR_PATH=$UPDATE_DIR/err.log + +echo "Check if need to start paparazzi ..." +START_PAPARAZZI=`grep start_paparazzi /data/config.ini | awk -F "=" '{ gsub(/ */,"",$2); print $2}'` +case $START_PAPARAZZI in +1) + START_PAPARAZZI=raw + ;; +2) + START_PAPARAZZI=sdk + ;; +*) + START_PAPARAZZI=no + ;; +esac +echo "START_PAPARAZZI=$START_PAPARAZZI" + +echo "Copy version.txt file in ftp directory" +cp /firmware/version.txt $VERSION_PATH + +PELF_ARGS=$(cat /tmp/.program.elf.arguments | tr '\n' ' ') + +echo "Check if update is necessary ..." +if [ -e $UPDATE_PATH ] ; then + VERSION=`cat $VERSION_PATH` + + if [ -e $ERR_PATH ] ; then + CHECK_ERR=`cat $ERR_PATH` + if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then + CHECK_PLF=`/bin/checkplf $UPDATE_PATH $VERSION` + if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then + echo "ERR=FLASH_KO" > $ERR_PATH + else + /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH + fi + else + /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH + fi + else + /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH + fi + + CHECK_ERR=`cat $ERR_PATH` + if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then + echo "File $UPDATE_PATH exists... Start updating..." + pinst_trigger + echo "Rebooting..." + reboot + else + if [ "$CHECK_ERR" = "VERSION_OK" ] ; then + echo "Version OK" + elif [ "$CHECK_ERR" = "ERR=FLASH_KO" ] ; then + echo "Error during Updating... Removing..." + else + echo "File $UPDATE_PATH not valid... Removing..." + fi +# Cleaning update directory + rm -Rf $UPDATE_DIR/* + cp /firmware/version.txt $VERSION_PATH + echo "Start Drone software..." + inetd + + # Check what to start + if [ "$START_PAPARAZZI" = "raw" ] ; then + (/data/video/raw/ap.elf; gpio 181 -d ho 1) & + elif [ "$START_PAPARAZZI" = "sdk" ] ; then + (/data/video/sdk/ap.elf; gpio 181 -d ho 1) & + else + (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & + fi + fi +else + echo "File $UPDATE_PATH doesn't exists... Start Drone software..." +# Cleaning update directory + rm -Rf $UPDATE_DIR/* + cp /firmware/version.txt $VERSION_PATH + + inetd + # Check what to start + if [ "$START_PAPARAZZI" = "raw" ] ; then + (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & + sleep 10 + (/data/video/raw/ap.elf; gpio 181 -d ho 1) & + elif [ "$START_PAPARAZZI" = "sdk" ] ; then + (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & + sleep 10 + (/data/video/sdk/ap.elf; gpio 181 -d ho 1) & + else + (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & + fi +fi diff --git a/sw/ext/ardrone2_drivers/wifi_setup.sh b/sw/ext/ardrone2_drivers/wifi_setup.sh new file mode 100755 index 0000000000..a1e075d98d --- /dev/null +++ b/sw/ext/ardrone2_drivers/wifi_setup.sh @@ -0,0 +1,201 @@ +#!/bin/sh +# +# Script to see if an IP adress is already used or not +# +# Getting SSID from config.ini file. + +#initializing random generator +cat /data/random_mac.txt > /dev/urandom +/bin/random_mac > /data/random_mac.txt + +#echo 2 > /proc/cpu/alignment + +export NETIF=ath0 +export WORKAREA="/lib/firmware" +export ATH_PLATFORM="parrot-omap-sdio" +export ATH_MODULE_ARGS="ifname=$NETIF" + +# Switch Wifi mode depending on value into /data/config.ini +# + +WIFI_MODE=`grep wifi_mode /data/config.ini | awk -F "=" '{ gsub(/ */,"",$2); print $2}'` + +case $WIFI_MODE in +0) + WIFI_MODE=master + ;; +1) + WIFI_MODE=ad-hoc + ;; +2) + WIFI_MODE=managed + ;; +*) + WIFI_MODE=master + ;; +esac + + +if [ -s /factory/mac_address.txt ] +then +MAC_ADDR=`cat /factory/mac_address.txt` +else +MAC_ADDR=`cat /data/random_mac.txt` +fi + +loadAR6000.sh -i $NETIF --setmac $MAC_ADDR + +# Waiting 2s for the wifi chip to be ready +sleep 2 + +AR6K_PID=`ps_procps -A -T -c | grep AR6K | awk '{print $1}'` +SDIO_PID=`ps_procps -A -T -c | grep ksdioirqd | awk '{print $1}'` + +#Changing wifi priority +chrt -p -r 25 $SDIO_PID +chrt -p -r 24 $AR6K_PID + + +# Disabling powersaving +wmiconfig -i $NETIF --power maxperf +# Disabling 802.11n aggregation +wmiconfig -i $NETIF --allow_aggr 0 0 +# enabling WMM +wmiconfig -i $NETIF --setwmm 1 + +i=0 + +while [ ! -n "$SSID" ] && [ $i -lt 10 ] +do +i=`expr $i + 1` +SSID=`grep ssid_single_player /data/config.ini | awk -F "=" '{print $2}'` + +# Removing leading and trailing spaces +SSID=`echo $SSID` +sleep 1 +done + +if [ -n "$SSID" ] +then +echo "SSID=$SSID" +else +#default SSID. +SSID=ardrone2_wifi +echo "SSID=\"$SSID\"" +fi + +RANDOM_CHAN=auto + + +echo "Creating $WIFI_MODE Network $SSID" + +iwconfig $NETIF mode $WIFI_MODE +iwconfig $NETIF essid "$SSID" + +if [ "$WIFI_MODE" != "managed" ] +then +# Allowing ACS to select only channels 1 & 6 +wmiconfig -i $NETIF --acsdisablehichannels 1 +iwconfig $NETIF channel $RANDOM_CHAN +iwconfig $NETIF rate auto +iwconfig $NETIF commit +else +# The Wifi connection freezes when in managed mode if there is no keepalive +wmiconfig -i ath0 --setkeepalive 1 +fi + +wmiconfig -i $NETIF --dtim 1 +wmiconfig -i $NETIF --commit + +OK=0 +BASE_ADRESS=`grep static_ip_address_base /data/config.ini | awk -F "=" '{print $2}'` +PROBE=`grep static_ip_address_probe /data/config.ini | awk -F "=" '{print $2}'` + +# Removing leading and trailing spaces +BASE_ADRESS=`echo $BASE_ADRESS` +PROBE=`echo $PROBE` + +# Default base address +if [ -n "$BASE_ADRESS" ] +then +echo "BASE_ADRESS=$BASE_ADRESS" +else +#default BASE_ADDRESS. +BASE_ADRESS=192.168.1. +echo "BASE_ADRESS=\"$BASE_ADRESS\"" +fi + +# Default probe +if [ -n "$PROBE" ] +then +echo "PROBE=$PROBE" +else +#default PROBE. +PROBE=1 +echo "PROBE=\"$PROBE\"" +fi + +while [ $OK -eq 0 ] +do +#configuring interface. +ifconfig $NETIF $BASE_ADRESS$PROBE +arping -I $NETIF -q -f -D -w 2 $BASE_ADRESS$PROBE + +if [ $? -eq 1 ] +then + if [ -s /data/old_adress.txt ] + then + # Testing previously given adress. + PROBE=`cat /data/old_adress.txt` + else + #generating random odd IP address + PROBE=`/bin/random_ip` + fi + /bin/random_ip > /data/old_adress.txt +else + echo $PROBE > /data/old_adress.txt + OK=1 +fi + +done + +#Configuring DHCP server. +echo "Using address $BASE_ADRESS$PROBE" +echo "start $BASE_ADRESS`expr $PROBE + 1`" > /tmp/udhcpd.conf +echo "end $BASE_ADRESS`expr $PROBE + 4`" >> /tmp/udhcpd.conf +echo "interface $NETIF" >> /tmp/udhcpd.conf +echo "decline_time 1" >> /tmp/udhcpd.conf +echo "conflict_time 1" >> /tmp/udhcpd.conf +echo "opt router $BASE_ADRESS$PROBE" >> /tmp/udhcpd.conf +echo "opt subnet 255.255.255.0" >> /tmp/udhcpd.conf +echo "opt lease 1200" >> /tmp/udhcpd.conf + +/bin/pairing_setup.sh + +# Saving random info for initialization at next reboot +echo $MAC_ADDR `date` `/bin/random_mac` > /dev/urandom +/bin/random_mac > /data/random_mac.txt + + + +telnetd -l /bin/sh + +# Check if not booting in master mode +if [ "$WIFI_MODE" != "managed" ] +then + udhcpd /tmp/udhcpd.conf +fi + +# Adding route for multicast-packet +route add -net 224.0.0.0 netmask 240.0.0.0 dev $NETIF + +# Allow reconnection to dirty TCP ports +echo "1" > /proc/sys/net/ipv4/tcp_tw_reuse +echo "1" > /proc/sys/net/ipv4/tcp_tw_recycle + +# Starting the daemon which responds to the AUTH messages from FreeFlight if not in master mode +if [ "$WIFI_MODE" != "managed" ] +then + /bin/parrotauthdaemon +fi + diff --git a/sw/ground_segment/cockpit/editFP.ml b/sw/ground_segment/cockpit/editFP.ml index 6b87f0c156..3a3673736f 100644 --- a/sw/ground_segment/cockpit/editFP.ml +++ b/sw/ground_segment/cockpit/editFP.ml @@ -50,7 +50,9 @@ let save_fp = fun geomap -> None -> () | Some file -> let f = open_out file in - fprintf f "\n\n"; + let fp_path = Str.replace_first (Str.regexp Env.flight_plans_path) "" (Filename.dirname file) in + let rel_path = Str.global_replace (Str.regexp (Printf.sprintf "%s[^%s]+" Filename.dir_sep Filename.dir_sep)) (Filename.parent_dir_name // "") fp_path in + fprintf f "\n\n" rel_path "flight_plan.dtd"; fprintf f "%s\n" (ExtXml.to_string_fmt fp#xml); close_out f; current_fp := Some (fp, file); 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/lib/ocaml/maps_support.ml b/sw/lib/ocaml/maps_support.ml index b6aacd5807..9d15ffe48c 100644 --- a/sw/lib/ocaml/maps_support.ml +++ b/sw/lib/ocaml/maps_support.ml @@ -24,9 +24,10 @@ let home = Env.paparazzi_home let (//) = Filename.concat let maps_xml_path = home // "conf" // "maps.xml" +let maps_xml_default_path = home // "conf" // "maps_example.xml" let maps_xml = ExtXml.parse_file maps_xml_path -let maps_xml_default = ExtXml.parse_file (maps_xml_path^".example") +let maps_xml_default = ExtXml.parse_file (maps_xml_default_path) let gv = try Some (ExtXml.int_attrib maps_xml "google_version") with _ -> None let gv_default = try ExtXml.int_attrib maps_xml_default "google_version" with _ -> 0 diff --git a/sw/simulator/nps/nps_atmosphere.c b/sw/simulator/nps/nps_atmosphere.c new file mode 100644 index 0000000000..d3aba4bfcf --- /dev/null +++ b/sw/simulator/nps/nps_atmosphere.c @@ -0,0 +1,61 @@ +/* + * 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 nps_atmosphere.c + * Atmosphere model (pressure, wind) for NPS. + */ + +#include "nps_atmosphere.h" +#include "nps_fdm.h" + +#ifndef NPS_QNH +#define NPS_QNH 101325.0 +#endif + +#ifndef NPS_WIND_SPEED +#define NPS_WIND_SPEED 0.0 +#endif + +#ifndef NPS_WIND_DIR +#define NPS_WIND_DIR 0 +#endif + +#ifndef NPS_TURBULENCE_SEVERITY +#define NPS_TURBULENCE_SEVERITY 0 +#endif + +struct NpsAtmosphere nps_atmosphere; + +void nps_atmosphere_init(void) { + + nps_atmosphere.qnh = NPS_QNH; + nps_atmosphere.wind_speed = NPS_WIND_SPEED; + nps_atmosphere.wind_dir = NPS_WIND_DIR; + nps_atmosphere.turbulence_severity = NPS_TURBULENCE_SEVERITY; + +} + +void nps_atmosphere_update(double dt __attribute__((unused))) { + nps_fdm_set_wind(nps_atmosphere.wind_speed, nps_atmosphere.wind_dir, + nps_atmosphere.turbulence_severity); +} + diff --git a/sw/simulator/nps/nps_atmosphere.h b/sw/simulator/nps/nps_atmosphere.h index d167c77dff..683db7e974 100644 --- a/sw/simulator/nps/nps_atmosphere.h +++ b/sw/simulator/nps/nps_atmosphere.h @@ -1,6 +1,46 @@ +/* + * 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 nps_atmosphere.h + * Atmosphere model (pressure, wind) for NPS. + */ + #ifndef NPS_ATMOSPHERE_H #define NPS_ATMOSPHERE_H +#include "math/pprz_algebra_double.h" + +struct NpsAtmosphere { + double qnh; ///< barometric pressure at sea level in Pascal + double wind_speed; ///< wind magnitude in m/s + double wind_dir; ///< wind direction in radians north=0, increasing CCW + int turbulence_severity; ///< turbulence severity from 0-7 +}; + +extern struct NpsAtmosphere nps_atmosphere; + +extern void nps_atmosphere_init(void); +extern void nps_atmosphere_update(double dt); + #endif /* NPS_ATMOSPHERE_H */ diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c new file mode 100644 index 0000000000..27826df4e3 --- /dev/null +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 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 "nps_autopilot_fixedwing.h" + +#ifdef FBW +#include "firmwares/fixedwing/main_fbw.h" +#define Fbw(f) f ## _fbw() +#else +#define Fbw(f) +#endif + +#ifdef AP +#include "firmwares/fixedwing/main_ap.h" +#define Ap(f) f ## _ap() +#else +#define Ap(f) +#endif + +#include "nps_sensors.h" +#include "nps_radio_control.h" +#include "nps_electrical.h" +#include "nps_fdm.h" + +#include "subsystems/radio_control.h" +#include "subsystems/imu.h" +#include "subsystems/sensors/baro.h" +#include "baro_board.h" +#include "mcu_periph/sys_time.h" +#include "state.h" +#include "subsystems/commands.h" + + + +struct NpsAutopilot autopilot; +bool_t nps_bypass_ahrs; + +#ifndef NPS_BYPASS_AHRS +#define NPS_BYPASS_AHRS FALSE +#endif + +#if !defined (FBW) || !defined (AP) +#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! +#endif + +void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) { + + nps_radio_control_init(type_rc, num_rc_script, rc_dev); + nps_electrical_init(); + + nps_bypass_ahrs = NPS_BYPASS_AHRS; + + Fbw(init); + Ap(init); + +} + +void nps_autopilot_run_systime_step( void ) { + sys_tick_handler(); +} + +#include +#include "subsystems/gps.h" + +void nps_autopilot_run_step(double time) { + + nps_electrical_run_step(time); + +#ifdef RADIO_CONTROL_TYPE_PPM + if (nps_radio_control_available(time)) { + radio_control_feed(); + Fbw(event_task); + } +#endif + + if (nps_sensors_gyro_available()) { + imu_feed_gyro_accel(); + Fbw(event_task); + Ap(event_task); + } + + if (nps_sensors_mag_available()) { + imu_feed_mag(); + Fbw(event_task); + Ap(event_task); + } + + if (nps_sensors_baro_available()) { + /** @todo feed baro values */ + //baro_feed_value(sensors.baro.value); + Fbw(event_task); + Ap(event_task); + } + + if (nps_sensors_gps_available()) { + gps_feed_value(); + Fbw(event_task); + Ap(event_task); + } + + if (nps_bypass_ahrs) { + sim_overwrite_ahrs(); + } + + Fbw(handle_periodic_tasks); + Ap(handle_periodic_tasks); + + /* scale final motor commands to 0-1 for feeding the fdm */ + for (uint8_t i=0; i < COMMANDS_NB; i++) + autopilot.commands[i] = (double)commands[i]/MAX_PPRZ; + +} + +void sim_overwrite_ahrs(void) { + + struct FloatQuat quat_f; + QUAT_COPY(quat_f, fdm.ltp_to_body_quat); + stateSetNedToBodyQuat_f(&quat_f); + + struct FloatRates rates_f; + RATES_COPY(rates_f, fdm.body_ecef_rotvel); + stateSetBodyRates_f(&rates_f); + +} diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.h b/sw/simulator/nps/nps_autopilot_fixedwing.h new file mode 100644 index 0000000000..a4d05a199b --- /dev/null +++ b/sw/simulator/nps/nps_autopilot_fixedwing.h @@ -0,0 +1,10 @@ +#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 f87e000747..6641cc5dc8 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -24,38 +24,45 @@ #include "firmwares/rotorcraft/main.h" #include "nps_sensors.h" #include "nps_radio_control.h" +#include "nps_electrical.h" +#include "nps_fdm.h" + #include "subsystems/radio_control.h" #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" #include "baro_board.h" -#include "subsystems/electrical.h" #include "mcu_periph/sys_time.h" #include "state.h" +#include "subsystems/ahrs.h" +#include "subsystems/ins.h" +#include "math/pprz_algebra.h" #include "subsystems/actuators/motor_mixing.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 + void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) { nps_radio_control_init(type_rc, num_rc_script, rc_dev); + nps_electrical_init(); + nps_bypass_ahrs = NPS_BYPASS_AHRS; + nps_bypass_ins = NPS_BYPASS_INS; main_init(); -#ifdef MAX_BAT_LEVEL - electrical.vsupply = MAX_BAT_LEVEL * 10; -#else - electrical.vsupply = 111; -#endif - } void nps_autopilot_run_systime_step( void ) { @@ -65,7 +72,9 @@ void nps_autopilot_run_systime_step( void ) { #include #include "subsystems/gps.h" -void nps_autopilot_run_step(double time __attribute__ ((unused))) { +void nps_autopilot_run_step(double time) { + + nps_electrical_run_step(time); #ifdef RADIO_CONTROL_TYPE_PPM if (nps_radio_control_available(time)) { @@ -98,6 +107,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { sim_overwrite_ahrs(); } + if (nps_bypass_ins) { + sim_overwrite_ins(); + } + handle_periodic_tasks(); /* scale final motor commands to 0-1 for feeding the fdm */ @@ -107,17 +120,31 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { } -#include "nps_fdm.h" -#include "subsystems/ahrs.h" -#include "math/pprz_algebra.h" + void sim_overwrite_ahrs(void) { - struct Int32Quat quat; - QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat); - stateSetNedToBodyQuat_i(&quat); + struct FloatQuat quat_f; + QUAT_COPY(quat_f, fdm.ltp_to_body_quat); + stateSetNedToBodyQuat_f(&quat_f); - struct Int32Rates rates; - RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel); - stateSetBodyRates_i(&rates); + struct FloatRates rates_f; + RATES_COPY(rates_f, fdm.body_ecef_rotvel); + 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_rotorcraft.h b/sw/simulator/nps/nps_autopilot_rotorcraft.h index 9d04b7213c..b71ffd7eb8 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.h +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.h @@ -5,6 +5,8 @@ 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_electrical.c b/sw/simulator/nps/nps_electrical.c new file mode 100644 index 0000000000..97e66eb908 --- /dev/null +++ b/sw/simulator/nps/nps_electrical.c @@ -0,0 +1,46 @@ +/* + * 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 nps_electrical.c + * Electrical status (bat voltage) for NPS. + */ + +#include "nps_electrical.h" +#include "generated/airframe.h" +#include "subsystems/electrical.h" + +struct NpsElectrical nps_electrical; + +void nps_electrical_init(void) { + +#ifdef MAX_BAT_LEVEL + nps_electrical.supply_voltage = MAX_BAT_LEVEL; +#else + nps_electrical.supply_voltage = 11.1; +#endif + +} + +void nps_electrical_run_step(double time __attribute__ ((unused))) { + // todo: auto-decrease bat voltage + electrical.vsupply = nps_electrical.supply_voltage * 10; +} diff --git a/sw/simulator/nps/nps_electrical.h b/sw/simulator/nps/nps_electrical.h new file mode 100644 index 0000000000..d1b15bcfdb --- /dev/null +++ b/sw/simulator/nps/nps_electrical.h @@ -0,0 +1,39 @@ +/* + * 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 nps_electrical.h + * Electrical status (bat voltage) for NPS. + */ + +#ifndef NPS_ELECTRICAL_H +#define NPS_ELECTRICAL_H + +struct NpsElectrical { + float supply_voltage; +}; + +extern struct NpsElectrical nps_electrical; + +extern void nps_electrical_init(void); +extern void nps_electrical_run_step(double time); + +#endif diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index 022364c0ce..99c0ec8691 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -43,6 +43,7 @@ struct NpsFdm { double init_dt; double curr_dt; bool_t on_ground; + int nan_count; /* position */ struct EcefCoor_d ecef_pos; @@ -85,11 +86,14 @@ struct NpsFdm { struct DoubleVect3 ltp_g; struct DoubleVect3 ltp_h; + struct DoubleVect3 wind; ///< velocity in m/s in NED + }; extern struct NpsFdm fdm; extern void nps_fdm_init(double dt); extern void nps_fdm_run_step(double* commands); +extern void nps_fdm_set_wind(double speed, double dir, int turbulence_severity); #endif /* NPS_FDM */ diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c index c2e5ebb95d..30a438d7e2 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.c +++ b/sw/simulator/nps/nps_fdm_jsbsim.c @@ -26,22 +26,42 @@ * This is an FDM for NPS that uses JSBSim as the simulation engine. */ +#include +#include +#include + #include #include #include #include #include -#include +#include + #include "nps_fdm.h" -#include "generated/airframe.h" #include "math/pprz_geodetic.h" #include "math/pprz_geodetic_double.h" #include "math/pprz_geodetic_float.h" #include "math/pprz_algebra.h" #include "math/pprz_algebra_float.h" +#include "generated/airframe.h" +#include "generated/flight_plan.h" + /// Macro to convert from feet to metres #define MetersOfFeet(_f) ((_f)/3.2808399) +#define FeetOfMeters(_m) ((_m)*3.2808399) + +/** Name of the JSBSim model. + * Defaults to the AIRFRAME_NAME + */ +#ifndef NPS_JSBSIM_MODEL +#define NPS_JSBSIM_MODEL AIRFRAME_NAME +#endif + +#ifdef NPS_INITIAL_CONDITITONS +#warning NPS_INITIAL_CONDITITONS was replaced by NPS_JSBSIM_INIT! +#warning Defaulting to flight plan location. +#endif /** Minimum JSBSim timestep * Around 1/10000 seems to be good for ground impacts @@ -49,9 +69,11 @@ #define MIN_DT (1.0/10240.0) using namespace JSBSim; +using namespace std; static void feed_jsbsim(double* commands); static void fetch_state(void); +static int check_for_nan(void); static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb_vector); static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location); @@ -60,7 +82,6 @@ static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_ static void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate); static void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate); static void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate); -//static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate); static void init_jsbsim(double dt); static void init_ltp(void); @@ -87,6 +108,8 @@ void nps_fdm_init(double dt) { for (min_dt = (1.0/dt); min_dt < (1/MIN_DT); min_dt += (1/dt)){} min_dt = (1/min_dt); + fdm.nan_count = 0; + init_jsbsim(dt); FDMExec->RunIC(); @@ -147,6 +170,24 @@ void nps_fdm_run_step(double* commands) { fetch_state(); + /* Check the current state to make sure it is valid (no NaNs) */ + if (check_for_nan()) { + printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time); + printf("It is likely the simulation diverged and gave non-physical results. If you did\n"); + printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n"); + exit(1); + } + +} + +void nps_fdm_set_wind(double speed, double dir, int turbulence_severity) { + FGWinds* Winds = FDMExec->GetWinds(); + Winds->SetWindspeed(FeetOfMeters(speed)); + Winds->SetWindPsi(dir); + + /* wind speed used for turbulence */ + Winds->SetWindspeed20ft(FeetOfMeters(speed)/2); + Winds->SetProbabilityOfExceedence(turbulence_severity); } /** @@ -169,6 +210,7 @@ static void feed_jsbsim(double* commands) { } + /** * Populates the NPS fdm struct after a simulation step. */ @@ -265,9 +307,15 @@ static void fetch_state(void) { /* * rotational speed and accelerations */ - jsbsimvec_to_rate(&fdm.body_ecef_rotvel,&propagate->GetPQR()); - jsbsimvec_to_rate(&fdm.body_ecef_rotaccel,&accelerations->GetPQRdot()); + jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR()); + jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot()); + + /* + * wind + */ + const FGColumnVector3& fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED(); + jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned); } /** @@ -284,9 +332,18 @@ static void init_jsbsim(double dt) { char buf[1024]; string rootdir; + string jsbsim_ic_name; sprintf(buf,"%s/conf/simulator/jsbsim/",getenv("PAPARAZZI_HOME")); rootdir = string(buf); + + /* if jsbsim initial conditions are defined, use them + * otherwise use flightplan location + */ +#ifdef NPS_JSBSIM_INIT + jsbsim_ic_name = NPS_JSBSIM_INIT; +#endif + FDMExec = new FGFDMExec(); FDMExec->Setsim_time(0.); @@ -298,7 +355,7 @@ static void init_jsbsim(double dt) { if ( ! FDMExec->LoadModel( rootdir + "aircraft", rootdir + "engine", rootdir + "systems", - AIRFRAME_NAME, + NPS_JSBSIM_MODEL, false)){ #ifdef DEBUG cerr << " JSBSim could not be started" << endl << endl; @@ -311,12 +368,39 @@ static void init_jsbsim(double dt) { FDMExec->GetPropulsion()->InitRunning(-1); JSBSim::FGInitialCondition *IC = FDMExec->GetIC(); - if ( ! IC->Load(NPS_INITIAL_CONDITITONS)) { + if(!jsbsim_ic_name.empty()) { + if ( ! IC->Load(jsbsim_ic_name)) { #ifdef DEBUG - cerr << "Initialization unsuccessful" << endl; + cerr << "Initialization unsuccessful" << endl; #endif - delete FDMExec; - exit(-1); + delete FDMExec; + exit(-1); + } + } + else { + // FGInitialCondition::SetAltitudeASLFtIC + // requires this function to be called + // before itself + IC->SetVgroundFpsIC(0.); + + // Use flight plan initial conditions + // convert geodetic lat from flight plan to geocentric + double gd_lat = RadOfDeg(NAV_LAT0 / 1e7); + double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT); + IC->SetLatitudeDegIC(DegOfRad(gc_lat)); + IC->SetLongitudeDegIC(NAV_LON0 / 1e7); + + IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0)); + IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT)); + IC->SetPsiDegIC(QFU); + IC->SetVgroundFpsIC(0.); + + //initRunning for all engines + FDMExec->GetPropulsion()->InitRunning(-1); + if (!FDMExec->RunIC()) { + cerr << "Initialization from flight plan unsuccessful" << endl; + exit(-1); + } } // calculate vehicle max radius in m @@ -473,13 +557,87 @@ void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate) { } - -#if 0 -static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate) { - - vector->x = rate->p; - vector->y = rate->q; - vector->z = rate->r; - -} +#ifdef __APPLE__ +/* Why isn't this there when we include math.h (on osx with clang)? */ +/// Check if a double is NaN. +static int isnan(double f) { return (f != f); } #endif + +/** + * Checks NpsFdm struct for NaNs. + * + * Increments the NaN count on each new NaN + * + * @return Count of new NaNs. 0 for no new NaNs. + */ +static int check_for_nan(void) { + int orig_nan_count = fdm.nan_count; + /* Check all elements for nans */ + if (isnan(fdm.ecef_pos.x)) fdm.nan_count++; + if (isnan(fdm.ecef_pos.y)) fdm.nan_count++; + if (isnan(fdm.ecef_pos.z)) fdm.nan_count++; + if (isnan(fdm.ltpprz_pos.x)) fdm.nan_count++; + if (isnan(fdm.ltpprz_pos.y)) fdm.nan_count++; + if (isnan(fdm.ltpprz_pos.z)) fdm.nan_count++; + if (isnan(fdm.lla_pos.lon)) fdm.nan_count++; + if (isnan(fdm.lla_pos.lat)) fdm.nan_count++; + if (isnan(fdm.lla_pos.alt)) fdm.nan_count++; + if (isnan(fdm.hmsl)) fdm.nan_count++; + // Skip debugging elements + if (isnan(fdm.ecef_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.body_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.body_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.body_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.body_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.body_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.body_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qi)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qx)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qy)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qz)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qi)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qx)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qy)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qz)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_eulers.phi)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_eulers.theta)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_eulers.psi)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qi)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qx)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qy)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qz)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_eulers.phi)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_eulers.theta)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_eulers.psi)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotvel.p)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotvel.q)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotvel.r)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotaccel.p)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotaccel.q)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotaccel.r)) fdm.nan_count++; + if (isnan(fdm.ltp_g.x)) fdm.nan_count++; + if (isnan(fdm.ltp_g.y)) fdm.nan_count++; + if (isnan(fdm.ltp_g.z)) fdm.nan_count++; + if (isnan(fdm.ltp_h.x)) fdm.nan_count++; + if (isnan(fdm.ltp_h.y)) fdm.nan_count++; + if (isnan(fdm.ltp_h.z)) fdm.nan_count++; + + return (fdm.nan_count - orig_nan_count); +} diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h index 64011b7a1f..db8b993365 100644 --- a/sw/simulator/nps/nps_ivy.h +++ b/sw/simulator/nps/nps_ivy.h @@ -1,6 +1,7 @@ #ifndef NPS_IVY #define NPS_IVY +extern void nps_ivy_common_init(char* ivy_bus); extern void nps_ivy_init(char* ivy_bus); extern void nps_ivy_display(void); diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy_common.c similarity index 85% rename from sw/simulator/nps/nps_ivy.c rename to sw/simulator/nps/nps_ivy_common.c index 93847c425c..4c869a22ac 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -9,8 +9,9 @@ #include "nps_autopilot.h" #include "nps_fdm.h" #include "nps_sensors.h" +#include "nps_atmosphere.h" #include "subsystems/ins.h" -#include "firmwares/rotorcraft/navigation.h" +#include "subsystems/navigation/common_flight_plan.h" #ifdef RADIO_CONTROL_TYPE_DATALINK #include "subsystems/radio_control.h" @@ -36,10 +37,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]); - #ifdef RADIO_CONTROL_TYPE_DATALINK static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), @@ -50,7 +47,7 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); #endif -void nps_ivy_init(char* ivy_bus) { +void nps_ivy_common_init(char* ivy_bus) { const char* agent_name = AIRFRAME_NAME"_NPS"; const char* ready_msg = AIRFRAME_NAME"_NPS Ready"; IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL); @@ -58,7 +55,6 @@ void nps_ivy_init(char* ivy_bus) { IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)"); IvyBindMsg(on_DL_GET_SETTING, NULL, "^(\\S*) GET_DL_SETTING (\\S*) (\\S*)"); IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)"); - IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); #ifdef RADIO_CONTROL_TYPE_DATALINK IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)"); @@ -117,25 +113,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), printf("goto block %d\n", block); } -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]) { - 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); -} - #ifdef RADIO_CONTROL_TYPE_DATALINK static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), @@ -210,4 +187,10 @@ void nps_ivy_display(void) { h_body.x, h_body.y, h_body.z); + + IvySendMsg("%d NPS_WIND %f %f %f", + AC_ID, + fdm.wind.x, + fdm.wind.y, + fdm.wind.z); } diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c new file mode 100644 index 0000000000..a7bd73cbba --- /dev/null +++ b/sw/simulator/nps/nps_ivy_fixedwing.c @@ -0,0 +1,56 @@ +#include "nps_ivy.h" + +#include +#include + +#include "generated/airframe.h" +#include "math/pprz_algebra_double.h" +#include "subsystems/ins.h" +#include "subsystems/navigation/common_nav.h" + +/* fixedwing specific Datalink Ivy functions */ +void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]); + +void nps_ivy_init(char* ivy_bus) { + /* init ivy and bind some messages common to fw and rotorcraft */ + nps_ivy_common_init(ivy_bus); + + IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); +} + +//TODO use datalink parsing from fixedwing instead of doing it here explicitly + +#include "generated/settings.h" +#include "dl_protocol.h" +#include "subsystems/datalink/downlink.h" + +#define MOfCm(_x) (((float)(_x))/100.) + +void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]) { + + if (atoi(argv[2]) == AC_ID) { + uint8_t wp_id = atoi(argv[1]); + float a = MOfCm(atoi(argv[5])); + + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(atoi(argv[3]) / 1e7)); + lla.lon = RadOfDeg((float)(atoi(argv[4]) / 1e7)); + //printf("move wp id=%d lat=%f lon=%f alt=%f\n", wp_id, lla.lat, lla.lon, a); + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, a); + + /* Waypoint range is limited. Computes the UTM pos back from the relative + coordinates */ + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); + printf("move wp id=%d east=%f north=%f zone=%i alt=%f\n", wp_id, utm.east, utm.north, utm.zone, a); + } +} diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c new file mode 100644 index 0000000000..bf0ef5562b --- /dev/null +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -0,0 +1,58 @@ +#include "nps_ivy.h" + +#include +#include + +#include "generated/airframe.h" +#include "math/pprz_algebra_double.h" +#include "nps_autopilot.h" +#include "nps_fdm.h" +#include "nps_sensors.h" +#include "subsystems/ins.h" +#include "firmwares/rotorcraft/navigation.h" + +#ifdef RADIO_CONTROL_TYPE_DATALINK +#include "subsystems/radio_control.h" +#endif + +#include NPS_SENSORS_PARAMS + + +/* rotorcraft specificDatalink Ivy functions */ +static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]); + +void nps_ivy_init(char* ivy_bus) { + /* init ivy and bind some messages common to fw and rotorcraft */ + nps_ivy_common_init(ivy_bus); + + IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); + +} + +//TODO use datalink parsing from rotorcraft instead of doing it here explicitly + +#include "generated/settings.h" +#include "dl_protocol.h" +#include "subsystems/datalink/downlink.h" +static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]) { + if (atoi(argv[2]) == AC_ID) { + 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); + } +} diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index a740eb15e0..6686beb324 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -84,7 +84,7 @@ double time_to_double(struct timeval *t) { return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6)); } -int main ( int argc, char** argv) { +int main (int argc, char** argv) { if (!nps_main_parse_options(argc, argv)) return 1; @@ -121,6 +121,7 @@ static void nps_main_init(void) { nps_ivy_init(nps_main.ivy_bus); nps_fdm_init(SIM_DT); + nps_atmosphere_init(); nps_sensors_init(nps_main.sim_time); printf("Simulating with dt of %f\n", SIM_DT); @@ -153,6 +154,8 @@ static void nps_main_init(void) { static void nps_main_run_sim_step(void) { // printf("sim at %f\n", nps_main.sim_time); + nps_atmosphere_update(SIM_DT); + nps_autopilot_run_systime_step(); nps_fdm_run_step(autopilot.commands); diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c index 746c04aed4..4b164777b2 100644 --- a/sw/simulator/nps/nps_radio_control_joystick.c +++ b/sw/simulator/nps/nps_radio_control_joystick.c @@ -27,13 +27,49 @@ * Simple DirectMedia Layer library is used for cross-platform support. * Joystick button and axes are mapped to RC commands directly with defines. * - * Currently it doesn't support different RC configurations. + * You must have a joystick with either: + * - 4 axes and 3 buttons, or + * - 5 axes + * + * First you should run sw/ground_segment/joystick/test_stick to determine + * the indices of the physical axes and buttons on your joystick (and to test + * that it actually works). Then you can assign each axis and button to a + * command in your airframe file. + * + * The default axes are Roll = 0, Pitch = 1, Yaw = 2 and Throttle = 3. + * The default buttons are Manual = 0, Auto1 = 1, Auto2 = 2. + * + * Example for 4 axes and 3 buttons using a joystick with 7 axes and 5 buttons: + * @verbatim + *
+ * ... + * + * + * + * + * + * + * + *
+ * @endverbatim + * + * One can define NPS_JS_AXIS_MODE to use an axis instead of buttons to change + * You will need a 5-axis joystick. + * + * If you need to reverse the direction of any axis, simply use: + * @verbatim + * + * + * @endverbatim + * + * At this point, no other functionality or channels are supported for R/C control. + * */ #include "nps_radio_control.h" #include "nps_radio_control_joystick.h" -// for NPS_JS_MODE_AXIS +// for NPS_JS_AXIS_MODE #include "generated/airframe.h" #include @@ -42,22 +78,37 @@ #include // axes indice -#define JS_ROLL 0 -#define JS_PITCH 1 -#define JS_YAW 2 -#define JS_THROTTLE 3 +#ifndef NPS_JS_AXIS_ROLL +#define NPS_JS_AXIS_ROLL 0 +#endif +#ifndef NPS_JS_AXIS_PITCH +#define NPS_JS_AXIS_PITCH 1 +#endif +#ifndef NPS_JS_AXIS_YAW +#define NPS_JS_AXIS_YAW 2 +#endif +#ifndef NPS_JS_AXIS_THROTTLE +#define NPS_JS_AXIS_THROTTLE 3 +#endif -#ifndef NPS_JS_MODE_AXIS +#ifndef NPS_JS_AXIS_MODE #define JS_NB_AXIS 4 #else #define JS_NB_AXIS 5 #endif // buttons to switch modes -#define JS_BUTTON_MODE_MANUAL 4 -#define JS_BUTTON_MODE_AUTO1 5 -#define JS_BUTTON_MODE_AUTO2 6 -#ifndef NPS_JS_MODE_AXIS +#ifndef NPS_JS_BUTTON_MODE_MANUAL +#define NPS_JS_BUTTON_MODE_MANUAL 1 +#endif +#ifndef NPS_JS_BUTTON_MODE_AUTO1 +#define NPS_JS_BUTTON_MODE_AUTO1 2 +#endif +#ifndef NPS_JS_BUTTON_MODE_AUTO2 +#define NPS_JS_BUTTON_MODE_AUTO2 3 +#endif + +#ifndef NPS_JS_AXIS_MODE #define JS_NB_BUTTONS 3 #else #define JS_NB_BUTTONS 0 @@ -132,6 +183,8 @@ int nps_radio_control_joystick_init(const char* device) { else if (SDL_JoystickNumAxes(sdl_joystick) < JS_NB_AXIS) { printf("Selected joystick does not support enough axes!\n"); + printf("Number of axes required: %i\n", JS_NB_AXIS); + printf("Number of axes available: %i\n",SDL_JoystickNumAxes(sdl_joystick)); SDL_JoystickClose(sdl_joystick); exit(-1); } @@ -155,13 +208,33 @@ int nps_radio_control_joystick_init(const char* device) { */ void nps_radio_control_joystick_update(void) { - nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,JS_THROTTLE)) - 32767.)/-65534.; - nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_ROLL))/32767.; - nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_PITCH))/32767.; - nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_YAW))/32767.; +#if NPS_JS_AXIS_THROTTLE_REVERSED + nps_joystick.throttle = ((float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.; +#else + nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.; +#endif +#if NPS_JS_AXIS_ROLL_REVERSED + nps_joystick.roll = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.; +#else + nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.; +#endif +#if NPS_JS_AXIS_PITCH_REVERSED + nps_joystick.pitch = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.; +#else + nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.; +#endif +#if NPS_JS_AXIS_YAW_REVERSED + nps_joystick.yaw = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.; +#else + nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.; +#endif // if an axis is asigned to the mode, use it instead of the buttons -#ifdef NPS_JS_MODE_AXIS - nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_MODE_AXIS))/32767.; +#ifdef NPS_JS_AXIS_MODE +#if NPS_JS_AXIS_MODE_REVERSED + nps_joystick.mode = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.; +#else + nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.; +#endif #endif while(SDL_PollEvent(&sdl_event)) @@ -172,16 +245,16 @@ void nps_radio_control_joystick_update(void) { { switch(sdl_event.jbutton.button) { -#ifndef NPS_JS_MODE_AXIS - case JS_BUTTON_MODE_MANUAL: +#ifndef NPS_JS_AXIS_MODE + case NPS_JS_BUTTON_MODE_MANUAL: nps_joystick.mode = MODE_SWITCH_MANUAL; break; - case JS_BUTTON_MODE_AUTO1: + case NPS_JS_BUTTON_MODE_AUTO1: nps_joystick.mode = MODE_SWITCH_AUTO1; break; - case JS_BUTTON_MODE_AUTO2: + case NPS_JS_BUTTON_MODE_AUTO2: nps_joystick.mode = MODE_SWITCH_AUTO2; break; #endif diff --git a/sw/supervision/pc_aircraft.ml b/sw/supervision/pc_aircraft.ml index 1c95fd3b9d..1537747e69 100644 --- a/sw/supervision/pc_aircraft.ml +++ b/sw/supervision/pc_aircraft.ml @@ -79,7 +79,8 @@ let new_ac_id = fun () -> let parse_conf_xml = fun vbox -> let strings = ref [] in Hashtbl.iter (fun name _ac -> strings := name :: !strings) Utils.aircrafts; - Gtk_tools.combo ("" :: !strings) vbox + let ordered = List.sort String.compare ("" :: !strings) in + Gtk_tools.combo ordered vbox let editor = try Sys.getenv "EDITOR" with _ -> ( diff --git a/sw/supervision/pc_control_panel.ml b/sw/supervision/pc_control_panel.ml index a476317842..2793706092 100644 --- a/sw/supervision/pc_control_panel.ml +++ b/sw/supervision/pc_control_panel.ml @@ -195,10 +195,11 @@ let supervision = fun ?file gui log (ac_combo : Gtk_tools.combo) (target_combo : Gtk_tools.add_to_combo session_combo "Simulation"; Gtk_tools.add_to_combo session_combo "Replay"; Gtk_tools.add_to_combo session_combo Gtk_tools.combo_separator; - Hashtbl.iter - (fun name _session -> - Gtk_tools.add_to_combo session_combo name) - sessions in + let strings = ref [] in + Hashtbl.iter (fun name _session -> strings := name :: !strings) sessions; + let ordered = List.sort String.compare !strings in + List.iter (fun name -> Gtk_tools.add_to_combo session_combo name) ordered + in register_custom_sessions (); Gtk_tools.select_in_combo session_combo "Simulation"; diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 2fb739ff18..082545f995 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -20,6 +20,7 @@ # Boston, MA 02111-1307, USA. # +from __future__ import print_function import sys import os @@ -29,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) @@ -42,6 +44,9 @@ def main(): parser.add_option("-p", "--plot", help="Show resulting plots", action="store_true", dest="plot") + parser.add_option("-a", "--auto_threshold", + help="Try to automatically determine noise threshold", + action="store_true", dest="auto_threshold") parser.add_option("-v", "--verbose", action="store_true", dest="verbose") (options, args) = parser.parse_args() @@ -54,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: @@ -65,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") @@ -84,14 +89,23 @@ 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 + if options.auto_threshold: + meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements])) + # set noise threshold to be below 10% of that + noise_threshold = meas_median * 0.1 + if options.verbose: + print("Using noise threshold of", noise_threshold, "for filtering.") # filter out noisy measurements flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold) if options.verbose: - print("remaining "+str(len(flt_meas))+" after low pass") + print("remaining "+str(len(flt_meas))+" after filtering") if len(flt_meas) == 0: - print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after low pass!") + print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after filtering!") sys.exit(1) # get an initial min/max guess 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 c079fd8bb9..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,20 +187,19 @@ 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] my = measured[:, 1] mz = measured[:, 2] - m_max = amax(abs(measured)) # calibrated values cx = calibrated[:, 0] @@ -224,17 +223,23 @@ def plot_mag_3d(measured, calibrated, p): ax.scatter(mx, my, mz) hold(True) # plot line from center to ellipsoid center - ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+') + ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+', markersize=10) # plot ellipsoid 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()) + # 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*') + ax.set_title('MAG raw with fitted ellipsoid and center offset') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') - ax.set_xlim3d(-m_max, m_max) - ax.set_ylim3d(-m_max, m_max) - ax.set_zlim3d(-m_max, m_max) if matplotlib.__version__.startswith('0'): ax = Axes3D(fig, rect=rect_r) @@ -267,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/dfu/stm32_mem.py b/sw/tools/dfu/stm32_mem.py index 48418433f2..7219e7efd3 100644 --- a/sw/tools/dfu/stm32_mem.py +++ b/sw/tools/dfu/stm32_mem.py @@ -136,6 +136,7 @@ if __name__ == "__main__": valid_manufacturers.append("Transition Robotics Inc.") valid_manufacturers.append("STMicroelectronics") valid_manufacturers.append("Black Sphere Technologies") + valid_manufacturers.append("TUDelft MavLab. 2012->13") # list of tuples with possible stm32 (autopilot) devices stm32devs = [] @@ -167,7 +168,7 @@ if __name__ == "__main__": if options.product == "any": stm32devs.append((dfudev, man, product, serial)) elif options.product == "Lisa/Lia": - if "Lisa/M" in product or "Lia" in product: + if "Lisa/M" in product or "Lia" in product or "Fireswarm" in product: stm32devs.append((dfudev, man, product, serial)) if not stm32devs: diff --git a/sw/tools/gen_aircraft.ml b/sw/tools/gen_aircraft.ml index e82a21c4c9..3065443de0 100644 --- a/sw/tools/gen_aircraft.ml +++ b/sw/tools/gen_aircraft.ml @@ -264,12 +264,13 @@ let dump_firmware_sections = fun xml makefile_ac -> (** Extracts the makefile sections of an airframe file *) -let extract_makefile = fun airframe_file makefile_ac -> +let extract_makefile = fun ac_id airframe_file makefile_ac -> let xml = Xml.parse_file airframe_file in let f = open_out makefile_ac in fprintf f "# This file has been generated from %s by %s\n" airframe_file Sys.argv.(0); fprintf f "# Please DO NOT EDIT\n"; + fprintf f "AC_ID=%s\n" ac_id; (** Search and dump makefile sections that have a "location" attribute set to "before" or no attribute *) dump_makefile_section xml f airframe_file "before"; @@ -397,7 +398,7 @@ let () = let temp_makefile_ac = Filename.temp_file "Makefile.ac" "tmp" in let abs_airframe_file = paparazzi_conf // airframe_file in - let modules_files = extract_makefile abs_airframe_file temp_makefile_ac in + let modules_files = extract_makefile (value "ac_id") abs_airframe_file temp_makefile_ac in (* Create Makefile.ac only if needed *) let makefile_ac = aircraft_dir // "Makefile.ac" in diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml index a8d0c34221..d18fead422 100644 --- a/sw/tools/gen_airframe.ml +++ b/sw/tools/gen_airframe.ml @@ -229,7 +229,7 @@ let parse_command = fun command no -> let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in { failsafe_value = failsafe_value; foo = 0} -let rec parse_section = fun s -> +let rec parse_section = fun ac_id s -> match Xml.tag s with "section" -> let prefix = ExtXml.attrib_or_default s "prefix" "" in @@ -281,11 +281,11 @@ let rec parse_section = fun s -> List.iter (fun d -> printf " Actuators%sInit();\\\n" d) drivers; printf "}\n\n"; | "include" -> - let filename = ExtXml.attrib s "href" in + let filename = Str.global_replace (Str.regexp "\\$AC_ID") ac_id (ExtXml.attrib s "href") in let subxml = Xml.parse_file filename in printf "/* XML %s */" filename; nl (); - List.iter parse_section (Xml.children subxml) + List.iter (parse_section ac_id) (Xml.children subxml) | "makefile" -> () (** Ignoring this section *) @@ -320,7 +320,7 @@ let _ = define "AC_ID" ac_id; define "MD5SUM" (sprintf "((uint8_t*)\"%s\")" (hex_to_bin md5sum)); nl (); - List.iter parse_section (Xml.children xml); + List.iter (parse_section ac_id) (Xml.children xml); finish h_name with Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1 diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index cdcdad33ac..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); diff --git a/sw/tools/gen_modules.ml b/sw/tools/gen_modules.ml index 1b4db5371f..7a32fcfe1c 100644 --- a/sw/tools/gen_modules.ml +++ b/sw/tools/gen_modules.ml @@ -344,9 +344,10 @@ let write_settings = fun xml_file out_set modules -> let h_name = "MODULES_H" let () = - if Array.length Sys.argv <> 3 then - failwith (Printf.sprintf "Usage: %s out_settings_file xml_file" Sys.argv.(0)); - let xml_file = Sys.argv.(2) + if Array.length Sys.argv <> 4 then + failwith (Printf.sprintf "Usage: %s out_settings_file default_freq xml_file" Sys.argv.(0)); + let xml_file = Sys.argv.(3) + and default_freq = int_of_string(Sys.argv.(2)) and out_set = open_out Sys.argv.(1) in try let xml = start_and_begin xml_file h_name in @@ -355,16 +356,18 @@ let () = fprintf out_h "#define MODULES_START 2\n"; fprintf out_h "#define MODULES_STOP 3\n"; nl (); + (* Extract main_freq parameter *) + let modules = try (ExtXml.child xml "modules") with _ -> Xml.Element("modules",[],[]) in + let main_freq = try (int_of_string (Xml.attrib modules "main_freq")) with _ -> default_freq in + freq := main_freq; + fprintf out_h "#define MODULES_FREQUENCY %d\n" !freq; + nl (); fprintf out_h "#ifdef MODULES_C\n"; fprintf out_h "#define EXTERN_MODULES\n"; fprintf out_h "#else\n"; fprintf out_h "#define EXTERN_MODULES extern\n"; fprintf out_h "#endif"; nl (); - (* Extract main_freq parameter *) - let modules = try (ExtXml.child xml "modules") with _ -> Xml.Element("modules",[],[]) in - let main_freq = try (int_of_string (Xml.attrib modules "main_freq")) with _ -> !freq in - freq := main_freq; (* Extract modules list *) let modules = GC.get_modules_of_airframe xml in let modules = GC.unload_unused_modules modules true in diff --git a/sw/tools/gen_periodic.ml b/sw/tools/gen_periodic.ml index 253182f8c3..1fdb99a7b4 100644 --- a/sw/tools/gen_periodic.ml +++ b/sw/tools/gen_periodic.ml @@ -142,6 +142,7 @@ let _ = fprintf out_h "#define _VAR_PERIODIC_H_\n\n"; fprintf out_h "#include \"std.h\"\n"; fprintf out_h "#include \"generated/airframe.h\"\n\n"; + fprintf out_h "#define TELEMETRY_FREQUENCY %d\n\n" freq; (** For each process *) List.iter diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 32313bd37a..e959b8e400 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -68,6 +68,12 @@ let print_dl_settings = fun settings -> lprintf "#include \"generated/periodic_telemetry.h\"\n"; lprintf "\n"; + (** Datalink knowing what settings mean **) + 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"; right (); diff --git a/tests/examples/01_compile_all_test_targets.t b/tests/examples/01_compile_all_test_targets.t index f18feddaa5..03bab305bf 100644 --- a/tests/examples/01_compile_all_test_targets.t +++ b/tests/examples/01_compile_all_test_targets.t @@ -8,7 +8,7 @@ use Data::Dumper; use Config; $|++; -my $examples = XMLin("$ENV{'PAPARAZZI_SRC'}/conf/tests_conf.xml"); +my $examples = XMLin("$ENV{'PAPARAZZI_SRC'}/conf/conf_tests.xml"); use Data::Dumper;