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 b845818f72..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 $< $@ @@ -294,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 diff --git a/conf/Makefile.omap b/conf/Makefile.omap index e567d0c314..9cc359d7cd 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -81,6 +81,38 @@ load upload program: $(OBJDIR)/$(TARGET).elf # Kill the application -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + # Make the target dir and edit the config + -{ \ + echo "mkdir -p $(TARGET_DIR)"; \ + } | telnet $(HOST) + + # Upload the drivers and new application + { \ + echo "binary"; \ + echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko"; \ + echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \ + echo "quit"; \ + } | ftp -n $(HOST) + + # Upload the modules and start the application + -{ \ + echo "insmod $(TARGET_DIR)/cdc-acm.ko"; \ + echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \ + echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \ + } | telnet $(HOST) + +ifeq ($(ARDRONE2_REBOOT),1) + -{ \ + echo "reboot"; \ + } | telnet $(HOST) +endif + +# Program the device and start it. +load2 upload2 program2: $(OBJDIR)/$(TARGET).elf + + # Kill the application + -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + # Make the target dir and edit the config -{ \ echo "mkdir -p $(TARGET_DIR)"; \ @@ -111,7 +143,7 @@ load upload program: $(OBJDIR)/$(TARGET).elf echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \ echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \ } | telnet $(HOST) - + ifeq ($(ARDRONE2_REBOOT),1) -{ \ echo "reboot"; \ @@ -119,6 +151,8 @@ ifeq ($(ARDRONE2_REBOOT),1) endif + + # Link: create ELF output file from object files. .SECONDARY : $(OBJDIR)/$(TARGET).elf .PRECIOUS : $(OBJ_C_OMAP) $(OBJ_CPP_OMAP) @@ -140,6 +174,7 @@ $(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac $(Q)test -d $(dir $@) || mkdir -p $(dir $@) $(Q)$(CXX) -c $(CXXFLAGS) $< -o $@ + # Listing of phony targets. .PHONY : all build elf clean clean_list diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml index 469c8e94b3..3a14a4338e 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml @@ -9,7 +9,7 @@ - + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml index f92bae4a6d..383607ba9b 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml @@ -2,14 +2,16 @@
- - - - - - - - + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml index 4b8a0a3b79..92af0c8aac 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml @@ -2,14 +2,19 @@
- - - - - - - - + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml index 688c81d4db..e3e8ef76bf 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml @@ -2,14 +2,19 @@
- - - - - - - - + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml index 688c81d4db..732f7e7065 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml @@ -2,14 +2,19 @@
- - - - - - - - + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml index 688c81d4db..d699f1a956 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml @@ -1,15 +1,15 @@ - +
- - - - - - - + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml index 688c81d4db..b5af215403 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml @@ -1,15 +1,15 @@ - +
- - - - - - - + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml index 688c81d4db..d6dd75562f 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml @@ -9,7 +9,7 @@ - + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml index 688c81d4db..8fd05e01ae 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml @@ -2,6 +2,11 @@
+ + + + + @@ -9,7 +14,7 @@ - + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml index 688c81d4db..14840c8c1e 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml @@ -2,14 +2,19 @@
- - - - - - - - + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml index 688c81d4db..36ef1433a2 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml @@ -1,15 +1,21 @@ - +
+ + + + + - - - - - - - + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml index 688c81d4db..294856f650 100644 --- a/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml +++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml @@ -2,14 +2,19 @@
- - - - - - - - + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index 8743fa786c..0542c01425 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -11,15 +11,15 @@ - + - + - + @@ -36,6 +36,8 @@ + + @@ -58,7 +60,7 @@ - + @@ -76,10 +78,6 @@
- - - - @@ -89,9 +87,9 @@
- - - + + +
@@ -181,10 +179,10 @@
- + - +
@@ -196,9 +194,9 @@
- - - + + +
diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index 35c2f7e1bb..39daec8c6b 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -55,12 +55,12 @@ - - - - - - + + + + + + @@ -71,12 +71,12 @@
- +
- +
@@ -106,29 +106,29 @@ - + - + - - + + - - - + + + - - - + + + @@ -154,10 +154,10 @@ - - - - + + + +
diff --git a/conf/airframes/TUDelft/IMAV2013/conf.xml b/conf/airframes/TUDelft/IMAV2013/conf.xml index 9c7ec3c285..dff3725763 100644 --- a/conf/airframes/TUDelft/IMAV2013/conf.xml +++ b/conf/airframes/TUDelft/IMAV2013/conf.xml @@ -1,22 +1,12 @@ - + + diff --git a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml index 7f719564ee..1dc80175b2 100644 --- a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml @@ -28,11 +28,14 @@ + + + @@ -40,10 +43,12 @@ - - + + + + - + @@ -77,7 +82,7 @@
- + diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index 31e1e248c8..8febddc167 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -50,12 +50,12 @@ - - - - - - + + + + + +
@@ -85,45 +85,43 @@ - - - + - - + + - + - - + + - + - - + + - + - + - - - + + + - + @@ -153,9 +151,9 @@
- - - + + +
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 851a85abfe..98ec28a287 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -5,7 +5,6 @@ - @@ -20,15 +19,14 @@ + - - @@ -39,7 +37,7 @@ - + @@ -55,11 +53,12 @@ + - - - - + + + +
@@ -71,36 +70,42 @@
- - - + + + + - - - - - - - - - + + + + + + + + + + + +
- - +
- -
- +
- + + + + + +
@@ -132,7 +137,6 @@
-
@@ -191,10 +195,13 @@
+ + + - +
@@ -217,5 +224,4 @@
- diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index 450b8469b1..48a6c58834 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -63,15 +63,16 @@ - - - + + +
+ - +
@@ -117,10 +118,10 @@ - + - + @@ -155,9 +156,9 @@ - - - + + +
@@ -168,9 +169,9 @@
- + - +
@@ -197,6 +198,7 @@ + diff --git a/conf/conf.xml.example b/conf/conf_example.xml similarity index 100% rename from conf/conf.xml.example rename to conf/conf_example.xml diff --git a/conf/tests_conf.xml b/conf/conf_tests.xml similarity index 100% rename from conf/tests_conf.xml rename to conf/conf_tests.xml diff --git a/conf/conf.xml.tri b/conf/conf_tri.xml similarity index 100% rename from conf/conf.xml.tri rename to conf/conf_tri.xml diff --git a/conf/control_panel.xml.example b/conf/control_panel_example.xml similarity index 100% rename from conf/control_panel.xml.example rename to conf/control_panel_example.xml diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile index ed7ce66951..3587d244df 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -53,6 +53,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_baro.c \ $(NPSDIR)/nps_sensor_gps.c \ $(NPSDIR)/nps_electrical.c \ + $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_radio_control.c \ $(NPSDIR)/nps_radio_control_joystick.c \ $(NPSDIR)/nps_radio_control_spektrum.c \ diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 45b618f08d..8db11612fd 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -49,6 +49,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_baro.c \ $(NPSDIR)/nps_sensor_gps.c \ $(NPSDIR)/nps_electrical.c \ + $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_radio_control.c \ $(NPSDIR)/nps_radio_control_joystick.c \ $(NPSDIR)/nps_radio_control_spektrum.c \ diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile index f9198a182c..9a1633ad9f 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile @@ -47,7 +47,7 @@ include $(CFG_SHARED)/imu_aspirin_v2_common.makefile # so that it will be unselected at init (baro CS line high) # ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1 +#IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1 else ifeq ($(ARCH), stm32) # SLAVE3 is on PC13, which is the baro CS IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE3 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/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 1e38778f87..435a83c755 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -48,6 +48,7 @@ + @@ -626,7 +627,11 @@ - + + + + + @@ -1860,7 +1865,13 @@ - + + + + + + + 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/settings/modules/flight_time.xml b/conf/settings/modules/flight_time.xml new file mode 100644 index 0000000000..312ec12ecc --- /dev/null +++ b/conf/settings/modules/flight_time.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml index 279caca01c..ba8aa8f8c6 100644 --- a/conf/settings/nps.xml +++ b/conf/settings/nps.xml @@ -6,7 +6,11 @@ + + + + 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/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c index 7f891b22c9..70e309b299 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c @@ -92,6 +92,9 @@ #endif +/** @todo: these should go into libopencm3 */ +#define TIM9 TIM9_BASE +#define TIM12 TIM12_BASE int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; @@ -102,10 +105,8 @@ static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, enum tim_oc_id oc_id) { timer_disable_oc_output(timer_peripheral, oc_id); -#if STM32F4 //There is no such register in TIM9 and 12. if (timer_peripheral != TIM9 && timer_peripheral != TIM12) -#endif 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); @@ -142,21 +143,15 @@ static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t chan * - Alignement edge. * - Direction up. */ -#if STM32F4 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 -#endif timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); // TIM1, 8 and 9 use APB2 clock, all others APB1 -#if STM32F4 if (timer != TIM1 && timer != TIM8 && timer != TIM9) { -#else - if (timer != TIM1 && timer != TIM8) { -#endif timer_set_prescaler(timer, (TIMER_APB1_CLK / ONE_MHZ_CLK) - 1); // 1uS } else { // TIM9, 1 and 8 use APB2 clock diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c index 56acd4d9cc..6ffc6a8f5a 100644 --- a/sw/airborne/boards/ardrone/electrical_raw.c +++ b/sw/airborne/boards/ardrone/electrical_raw.c @@ -69,7 +69,8 @@ int fd; void electrical_init(void) { // First we try to kill the program.elf if it is running (done here because initializes first) - system("killall -9 program.elf"); + int ret = system("killall -9 program.elf"); + (void) ret; // Initialize 12c device for power fd = open( "/dev/i2c-1", O_RDWR ); diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 1ddd68a5aa..e4f43a136a 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -36,26 +36,26 @@ #include #include #include + +#include "std.h" #include "navdata.h" +#include "subsystems/ins.h" #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; - -static navdata_port port; -static int nav_fd; +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) { @@ -100,15 +100,15 @@ static void navdata_write(const uint8_t *buf, size_t count) perror("navdata_write: Write failed"); } -int navdata_init() +bool_t navdata_init() { - nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); - if (nav_fd == -1) - { - perror("navdata_init: Unable to open /dev/ttyO1 - "); - return 1; - } else { - port.isOpen = 1; + 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 @@ -132,31 +132,40 @@ int navdata_init() uint8_t cmd=0x02; 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; + cmd = 0x01; navdata_write(&cmd, 1); - 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) { - char tmp[100]; - read(nav_fd, tmp, sizeof tmp); // read some potential dirt - // start baro calibration acquisition uint8_t cmd=0x17; // send cmd 23 navdata_write(&cmd, 1); @@ -166,7 +175,7 @@ void acquire_baro_calibration() if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) { perror("acquire_baro_calibration: read failed"); - return; + return FALSE; } baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; @@ -195,32 +204,19 @@ 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; } } @@ -235,7 +231,7 @@ static 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) @@ -244,16 +240,16 @@ static void baro_update_logic(void) 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) @@ -262,37 +258,52 @@ static void baro_update_logic(void) 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; } 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 >= NAVDATA_PACKET_SIZE) + 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 ) - { - assert(sizeof navdata == NAVDATA_PACKET_SIZE); - 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); @@ -306,87 +317,59 @@ void navdata_update() 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(NAVDATA_PACKET_SIZE); + + // 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() { - +int16_t navdata_height(void) { if (navdata.ultrasound > 10000) { return previousUltrasoundHeight; } int16_t ultrasoundHeight = 0; - 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 dd3b36f68b..f41b52a9c0 100644 --- a/sw/airborne/boards/ardrone/navdata.h +++ b/sw/airborne/boards/ardrone/navdata.h @@ -95,25 +95,29 @@ struct bmp180_baro_calibration int32_t b5; }; +#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); - -uint16_t navdata_checksum(void); -int16_t navdata_getHeight(void); - -void acquire_baro_calibration(void); +int16_t navdata_height(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); diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 4a78cdfad4..adf1490c9b 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -129,21 +129,34 @@ void autopilot_init(void) { 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_ground_detected) { - autopilot_set_mode(AP_MODE_KILL); - autopilot_ground_detected = 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 { @@ -171,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); @@ -215,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: diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 412421cd60..7d6d6423b5 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -646,6 +646,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, \ @@ -983,7 +991,8 @@ &navdata.mx, \ &navdata.my, \ &navdata.mz, \ - &navdata.chksum \ + &navdata.chksum, \ + &nav_port.checksum_errors \ ) #else #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {} @@ -998,10 +1007,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 @@ -1011,10 +1023,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 @@ -1024,10 +1039,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 @@ -1037,10 +1055,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_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/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/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 cb5d101555..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; diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 549630a8d5..546d32d549 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -29,6 +29,7 @@ #endif bool_t gps_available; +bool_t gps_has_fix; void gps_feed_value() { gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; @@ -75,10 +76,14 @@ void gps_feed_value() { gps.utm_pos.zone = nav_utm_zone0; #endif - gps.fix = GPS_FIX_3D; + 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_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index 207b0df6ab..ef708b53ee 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -176,9 +176,21 @@ void imu_aspirin2_event(void) 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); 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/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/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_electrical.c b/sw/simulator/nps/nps_electrical.c index bc4c3e520a..97e66eb908 100644 --- a/sw/simulator/nps/nps_electrical.c +++ b/sw/simulator/nps/nps_electrical.c @@ -19,6 +19,10 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file nps_electrical.c + * Electrical status (bat voltage) for NPS. + */ #include "nps_electrical.h" #include "generated/airframe.h" diff --git a/sw/simulator/nps/nps_electrical.h b/sw/simulator/nps/nps_electrical.h index a8f6054814..d1b15bcfdb 100644 --- a/sw/simulator/nps/nps_electrical.h +++ b/sw/simulator/nps/nps_electrical.h @@ -19,6 +19,11 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file nps_electrical.h + * Electrical status (bat voltage) for NPS. + */ + #ifndef NPS_ELECTRICAL_H #define NPS_ELECTRICAL_H diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index 4332fe0104..99c0ec8691 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -86,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 4487ae8c14..30a438d7e2 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.c +++ b/sw/simulator/nps/nps_fdm_jsbsim.c @@ -35,6 +35,7 @@ #include #include #include +#include #include "nps_fdm.h" #include "math/pprz_geodetic.h" @@ -179,6 +180,16 @@ void nps_fdm_run_step(double* commands) { } +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); +} + /** * Feed JSBSim with the latest actuator commands. * @@ -199,6 +210,7 @@ static void feed_jsbsim(double* commands) { } + /** * Populates the NPS fdm struct after a simulation step. */ @@ -295,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); } /** diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c index c93bd7d4eb..4c869a22ac 100644 --- a/sw/simulator/nps/nps_ivy_common.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -9,6 +9,7 @@ #include "nps_autopilot.h" #include "nps_fdm.h" #include "nps_sensors.h" +#include "nps_atmosphere.h" #include "subsystems/ins.h" #include "subsystems/navigation/common_flight_plan.h" @@ -186,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_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/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py index c079fd8bb9..0cc8f54b75 100644 --- a/sw/tools/calibration/calibration_utils.py +++ b/sw/tools/calibration/calibration_utils.py @@ -200,7 +200,6 @@ def plot_mag_3d(measured, calibrated, p): mx = measured[:, 0] my = measured[:, 1] mz = measured[:, 2] - m_max = amax(abs(measured)) # calibrated values cx = calibrated[:, 0] @@ -224,17 +223,24 @@ 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) 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_airframe.ml b/sw/tools/gen_airframe.ml index 8e066b3ec4..d18fead422 100644 --- a/sw/tools/gen_airframe.ml +++ b/sw/tools/gen_airframe.ml @@ -281,7 +281,7 @@ let rec parse_section = fun ac_id s -> List.iter (fun d -> printf " Actuators%sInit();\\\n" d) drivers; printf "}\n\n"; | "include" -> - let filename = Str.global_replace (Str.regexp "\$AC_ID") ac_id (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 (); diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index cdcdad33ac..62449a3be6 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -794,6 +794,15 @@ let () = Xml2h.define "HOME_MODE_HEIGHT" (sof home_mode_height); Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh); + (** Print defines for blocks **) + lprintf "\n"; + let idx = ref 0 in + List.iter + (fun s -> + let v = ExtXml.attrib s "name" in + lprintf "#define BLOCK_%s %d\n" (Str.global_replace (Str.regexp "[^A-Za-z0-9]") "_" v) !idx; incr idx) blocks; + lprintf "\n"; + let index_of_waypoints = let i = ref (-1) in List.map (fun w -> incr i; (name_of w, !i)) waypoints in diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 32313bd37a..2413805309 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -68,6 +68,16 @@ let print_dl_settings = fun settings -> lprintf "#include \"generated/periodic_telemetry.h\"\n"; lprintf "\n"; + (** Datalink knowing what settings mean **) + let idx = ref 0 in + lprintf "\n"; + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "#define SETTINGS_%s %d\n" (Str.global_replace (Str.regexp "[^A-Za-z0-9]") "_" v) !idx; incr idx) + settings; + lprintf "\n"; + (** 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;