diff --git a/.gitignore b/.gitignore index 3f6f5456a1..6a59db1e0e 100644 --- a/.gitignore +++ b/.gitignore @@ -31,6 +31,9 @@ /build.properties /META-INF +# JetBrains (PyCharm, etc) IDE project files +.idea + /var /dox diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index 8aaaeddef4..fa39dbb7cf 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -227,101 +227,11 @@ $(AOBJ) : $(OBJDIR)/%.o : %.S $(Q)test -d $(dir $@) || mkdir -p $(dir $@) $(Q)$(CC) -c $(AFLAGS) $< -o $@ -# -# check which flash mode is configured -# -ifeq ($(FLASH_MODE),DFU) -ifeq ($(DFU_UTIL),y) -# -# DFU flash mode using dfu-util -DFU_ADDR ?= 0x08000000 -upload: $(OBJDIR)/$(TARGET).bin - @echo "Using dfu-util at $(DFU_ADDR)" - $(Q)dfu-util -d 0483:df11 -c 1 -i 0 -a 0 -s $(DFU_ADDR) -D $^ -else -# -# DFU flash mode paparazzi stm32_mem -ifdef DFU_ADDR -DFU_ADDR_CMD = --addr=$(DFU_ADDR) -endif -ifdef DFU_PRODUCT -DFU_PRODUCT_CMD = --product=$(DFU_PRODUCT) -endif -upload: $(OBJDIR)/$(TARGET).bin - @echo "Using stm32 mem dfu loader" - $(PYTHON) $(PAPARAZZI_SRC)/sw/tools/dfu/stm32_mem.py $(DFU_PRODUCT_CMD) $(DFU_ADDR_CMD) $^ -endif - -# -# serial flash mode -else ifeq ($(FLASH_MODE),SERIAL) -upload: $(OBJDIR)/$(TARGET).bin - $(LOADER) -p /dev/ttyUSB0 -b 115200 -e -w -v $^ -# -# JTAG flash mode -else ifeq ($(FLASH_MODE),JTAG) -# either via normal jtag or BlackMagicProbe -ifeq ($(BMP_PORT),) -# normal jtag via OpenOCD -upload: $(OBJDIR)/$(TARGET).hex - @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" - @echo "Using OOCD = $(OOCD)" - @echo " OOCD\t$<" - $(Q)$(OOCD) -f interface/$(OOCD_INTERFACE).cfg \ - -f board/$(OOCD_BOARD).cfg $(OOCD_OPTIONS) \ - -c init \ - -c "reset halt" \ - -c "reset init" \ - -c "flash erase_sector 0 $(OOCD_START_SECTOR) last" \ - -c "flash write_image $<" \ - -c reset \ - -c shutdown -else -# jtag via BMP -BMP_UPLOAD_SCRIPT ?= $(PAPARAZZI_SRC)/sw/tools/flash_scripts/bmp_jtag_flash.scr - -upload: $(OBJDIR)/$(TARGET).elf - @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" - @echo "Using Black Magic Probe with JTAG on BMP_PORT $(BMP_PORT)" - @echo "Using GDB = $(GDB)" - @echo " BMP\t$<" - $(Q)$(GDB) --batch \ - -ex 'target extended-remote $(BMP_PORT)' \ - -x $(BMP_UPLOAD_SCRIPT) \ - $< -endif -# -# SWD flash mode -else ifeq ($(FLASH_MODE),SWD) -# only works if BMP_PORT is defined -ifeq ($(STLINK),y) -STLINK_ADDR ?= 0x08000000 -upload: $(OBJDIR)/$(TARGET).bin - @echo "Using ST-LINK with SWD at $(STLINK_ADDR)" - $(Q)st-flash write $^ $(STLINK_ADDR) -else -BMP_PORT ?= /dev/ttyACM0 -BMP_UPLOAD_SCRIPT ?= $(PAPARAZZI_SRC)/sw/tools/flash_scripts/bmp_swd_flash.scr -upload: $(OBJDIR)/$(TARGET).elf - @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" - @echo "Using Black Magic Probe with SWD on BMP_PORT $(BMP_PORT)" - @echo "Using GDB = $(GDB)" - @echo " BMP\t$<" - $(Q)$(GDB) --batch \ - -ex 'target extended-remote $(BMP_PORT)' \ - -x $(BMP_UPLOAD_SCRIPT) \ - $< -endif -# -# no known flash mode -else -upload: - @echo unknown flash_mode $(FLASH_MODE) -endif - +# Load upload rules +include $(PAPARAZZI_SRC)/conf/Makefile.stm32-upload # Listing of phony targets. -.PHONY : all build elf bin lss sym upload start_gdb start_telnet +.PHONY : all build elf bin lss sym start_gdb start_telnet # # Dependencies diff --git a/conf/Makefile.stm32-upload b/conf/Makefile.stm32-upload new file mode 100644 index 0000000000..2c960fb1bb --- /dev/null +++ b/conf/Makefile.stm32-upload @@ -0,0 +1,119 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# 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, see +# . +# + +# +# This is the common Makefile for STM32 upload rules +# + +# +# check which flash mode is configured +# +ifeq ($(FLASH_MODE),DFU) +ifeq ($(DFU_UTIL),y) +# +# DFU flash mode using dfu-util +DFU_ADDR ?= 0x08000000 +upload: $(OBJDIR)/$(TARGET).bin + @echo "Using dfu-util at $(DFU_ADDR)" + $(Q)dfu-util -d 0483:df11 -c 1 -i 0 -a 0 -s $(DFU_ADDR) -D $^ +else +# +# DFU flash mode paparazzi stm32_mem +ifdef DFU_ADDR +DFU_ADDR_CMD = --addr=$(DFU_ADDR) +endif +ifdef DFU_PRODUCT +DFU_PRODUCT_CMD = --product=$(DFU_PRODUCT) +endif +upload: $(OBJDIR)/$(TARGET).bin + @echo "Using stm32 mem dfu loader" + $(PYTHON) $(PAPARAZZI_SRC)/sw/tools/dfu/stm32_mem.py $(DFU_PRODUCT_CMD) $(DFU_ADDR_CMD) $^ +endif + +# +# serial flash mode +else ifeq ($(FLASH_MODE),SERIAL) +upload: $(OBJDIR)/$(TARGET).bin + $(LOADER) -p /dev/ttyUSB0 -b 115200 -e -w -v $^ +# +# JTAG flash mode +else ifeq ($(FLASH_MODE),JTAG) +# either via normal jtag or BlackMagicProbe +ifeq ($(BMP_PORT),) +# normal jtag via OpenOCD +upload: $(OBJDIR)/$(TARGET).hex + @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" + @echo "Using OOCD = $(OOCD)" + @echo " OOCD\t$<" + $(Q)$(OOCD) -f interface/$(OOCD_INTERFACE).cfg \ + -f board/$(OOCD_BOARD).cfg $(OOCD_OPTIONS) \ + -c init \ + -c "reset halt" \ + -c "reset init" \ + -c "flash erase_sector 0 $(OOCD_START_SECTOR) last" \ + -c "flash write_image $<" \ + -c reset \ + -c shutdown +else +# jtag via BMP +BMP_UPLOAD_SCRIPT ?= $(PAPARAZZI_SRC)/sw/tools/flash_scripts/bmp_jtag_flash.scr + +upload: $(OBJDIR)/$(TARGET).elf + @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" + @echo "Using Black Magic Probe with JTAG on BMP_PORT $(BMP_PORT)" + @echo "Using GDB = $(GDB)" + @echo " BMP\t$<" + $(Q)$(GDB) --batch \ + -ex 'target extended-remote $(BMP_PORT)' \ + -x $(BMP_UPLOAD_SCRIPT) \ + $< +endif +# +# SWD flash mode +else ifeq ($(FLASH_MODE),SWD) +# only works if BMP_PORT is defined +ifeq ($(STLINK),y) +STLINK_ADDR ?= 0x08000000 +upload: $(OBJDIR)/$(TARGET).bin + @echo "Using ST-LINK with SWD at $(STLINK_ADDR)" + $(Q)st-flash write $^ $(STLINK_ADDR) +else +BMP_PORT ?= /dev/ttyACM0 +BMP_UPLOAD_SCRIPT ?= $(PAPARAZZI_SRC)/sw/tools/flash_scripts/bmp_swd_flash.scr +upload: $(OBJDIR)/$(TARGET).elf + @echo "Assuming luftboot bootloader: $(ASSUMING_LUFTBOOT)" + @echo "Using Black Magic Probe with SWD on BMP_PORT $(BMP_PORT)" + @echo "Using GDB = $(GDB)" + @echo " BMP\t$<" + $(Q)$(GDB) --batch \ + -ex 'target extended-remote $(BMP_PORT)' \ + -x $(BMP_UPLOAD_SCRIPT) \ + $< +endif +# +# no known flash mode +else +upload: + @echo unknown flash_mode $(FLASH_MODE) +endif + +.PHONY : upload + diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index b5ca327b07..7e0bfc0318 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -216,6 +216,7 @@
+
diff --git a/conf/airframes/ENAC/fixed-wing/crrcsim.xml b/conf/airframes/ENAC/fixed-wing/crrcsim.xml new file mode 100644 index 0000000000..475815919d --- /dev/null +++ b/conf/airframes/ENAC/fixed-wing/crrcsim.xml @@ -0,0 +1,258 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + +
+ +
+ +
+ +
+ + + + + + + +
+ + diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 1d1a729e9a..632b993672 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -257,6 +257,7 @@
+
diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index 0542c01425..296ce37929 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -92,10 +92,6 @@ -
- -
-
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml index af754f073d..37ce741d63 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 @@ -249,6 +249,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 a474774180..943f4708df 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 @@ -249,6 +249,7 @@
+
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 4378808f12..5d91ec3e6d 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -189,10 +189,6 @@ - - - -
@@ -210,6 +206,7 @@
+
diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index 72a05e09bc..a53bf728f1 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -190,6 +190,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 ffdede1e73..1d854cd414 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -214,6 +214,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 4a3944ecb8..7d398ef073 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -212,6 +212,7 @@
+
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 1440ea0781..cf4b170221 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -175,6 +175,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 b332a0c730..ed03218407 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -199,6 +199,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 136a0b1d02..ee7a9f1bf8 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 @@ -200,6 +200,7 @@
+
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 016537f4f4..0001b425c2 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -189,6 +189,7 @@
+
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index c963633c5e..0ba0db9545 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -180,6 +180,7 @@
+
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 5c264fc3a7..9a2048e390 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -198,6 +198,7 @@
+ diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 3316eec5ff..194bba93bb 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -264,6 +264,7 @@
+
diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index b189c2c8e6..2fbcb261f4 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -194,6 +194,7 @@
+
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 9139db0e35..b9e6bac2ab 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -235,6 +235,7 @@
+ diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile new file mode 100644 index 0000000000..096c5a7327 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile @@ -0,0 +1,52 @@ +# Hey Emacs, this is a -*- makefile -*- + +# +# SITL Simulator based on CRRCSIM (MNAV inputdev) +# + +SRC_FIRMWARE=firmwares/fixedwing + +SRC_BOARD=boards/$(BOARD) + +NPSDIR = $(SIMDIR)/nps + + +nps.ARCHDIR = sim + +# include Makefile.nps instead of Makefile.sim +nps.MAKEFILE = nps + +# add normal ap and fbw sources define in autopilot.makefile +nps.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS) +nps.srcs += $(fbw_srcs) $(ap_srcs) + +nps.CFLAGS += -DSITL -DUSE_NPS +nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) +nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lpcre -lgsl -lgslcblas +nps.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps +nps.LDFLAGS += $(shell sdl-config --libs) + +nps.srcs += $(NPSDIR)/nps_main.c \ + $(NPSDIR)/nps_fdm_crrcsim.c \ + $(NPSDIR)/nps_random.c \ + $(NPSDIR)/nps_sensors.c \ + $(NPSDIR)/nps_sensors_utils.c \ + $(NPSDIR)/nps_sensor_gyro.c \ + $(NPSDIR)/nps_sensor_accel.c \ + $(NPSDIR)/nps_sensor_mag.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 \ + $(NPSDIR)/nps_autopilot_fixedwing.c \ + $(NPSDIR)/nps_ivy_common.c \ + $(NPSDIR)/nps_ivy_fixedwing.c \ + $(NPSDIR)/nps_flightgear.c \ + + +nps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport +nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c + diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile index e12acb6c6a..84684937f4 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile @@ -6,6 +6,7 @@ GX3_BAUD ?= B921600 AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS +AHRS_CFLAGS += -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_IMU AHRS_CFLAGS += -DUSE_IMU_FLOAT AHRS_CFLAGS += -DUSE_GX3 diff --git a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile index 17c50f6ba2..e51c679055 100644 --- a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile +++ b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile @@ -59,9 +59,11 @@ else ifeq ($(ARCH), stm32) DROTEK_2_I2C_DEV ?= i2c2 endif +ifeq ($(TARGET), ap) ifndef DROTEK_2_I2C_DEV $(error Error: DROTEK_2_I2C_DEV not configured!) endif +endif # convert i2cx to upper/lower case DROTEK_2_I2C_DEV_UPPER=$(shell echo $(DROTEK_2_I2C_DEV) | tr a-z A-Z) diff --git a/conf/firmwares/subsystems/shared/imu_gl1.makefile b/conf/firmwares/subsystems/shared/imu_gl1.makefile index 18107c5c9f..c43a1d8d8a 100644 --- a/conf/firmwares/subsystems/shared/imu_gl1.makefile +++ b/conf/firmwares/subsystems/shared/imu_gl1.makefile @@ -28,9 +28,11 @@ else ifeq ($(ARCH), stm32) GL1_I2C_DEV ?= i2c2 endif +ifeq ($(TARGET), ap) ifndef GL1_I2C_DEV $(error Error: GL1_I2C_DEV not configured!) endif +endif # convert i2cx to upper/lower case GL1_I2C_DEV_UPPER=$(shell echo $(GL1_I2C_DEV) | tr a-z A-Z) diff --git a/conf/settings/control/stabilization_att_float_euler.xml b/conf/settings/control/stabilization_att_float_euler.xml new file mode 100644 index 0000000000..0cf9eb8c83 --- /dev/null +++ b/conf/settings/control/stabilization_att_float_euler.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml b/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml deleted file mode 100644 index 221cd780b1..0000000000 --- a/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml +++ /dev/null @@ -1,396 +0,0 @@ - - - - - - Gustavo Violato & Antoine Drouin - 24-02-2009 - Version 0.9 - beta - Simple Quadrotor without rotor dynamic - - - - 78.53 - 10 - 6.89 - 0 - 0 - 0 - 0 - - 0 - 0 - 0 - - - 0 - 0 - 0 - - - 0 - 0 - 0 - - - - - 0.005 - 0.005 - 0.010 - 0. - 0. - 0. - 0.84 - - 0 - 0 - 0 - - - - - - - -0.15 - 0 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0.15 - 0 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0. - 0.15 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0. - -0.15 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - - - - - - - - - fcs/front_motor - fcs/back_motor - fcs/right_motor - fcs/left_motor - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - 0 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - 0 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/right_motor - 0.84 - - - - 0 - 6.89 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/left_motor - 0.84 - - - - 0 - -6.89 - 0 - - - 0 - 0 - -1 - - - - - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - - 1.9685 - 0 - - - 1 - 0 - 0 - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - -1.9685 - 0 - - - -1 - 0 - 0 - - - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - 1.9685 - 0 - - - 1 - 0 - 0 - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - -1.9685 - 0 - - - -1 - 0 - 0 - - - - - - - - - fcs/right_motor - 0.84 - - - - -1.9685 - 6.89 - 0 - - - 0 - 1 - 0 - - - - - - - fcs/right_motor - 0.84 - - - - 1.9685 - 6.89 - 0 - - - 0 - -1 - 0 - - - - - - - - - fcs/left_motor - 0.84 - - - - -1.9685 - -6.89 - 0 - - - 0 - 1 - 0 - - - - - - - fcs/left_motor - 0.84 - - - - 1.9685 - -6.89 - 0 - - - 0 - -1 - 0 - - - - - - - - - - - - - Drag - - aero/qbar-psf - 47.9 - 0.0151 - 0.224808943 - - - - - - diff --git a/conf/simulator/jsbsim/aircraft/BOOZ2_A1P.xml b/conf/simulator/jsbsim/aircraft/BOOZ2_A1P.xml deleted file mode 100644 index 221cd780b1..0000000000 --- a/conf/simulator/jsbsim/aircraft/BOOZ2_A1P.xml +++ /dev/null @@ -1,396 +0,0 @@ - - - - - - Gustavo Violato & Antoine Drouin - 24-02-2009 - Version 0.9 - beta - Simple Quadrotor without rotor dynamic - - - - 78.53 - 10 - 6.89 - 0 - 0 - 0 - 0 - - 0 - 0 - 0 - - - 0 - 0 - 0 - - - 0 - 0 - 0 - - - - - 0.005 - 0.005 - 0.010 - 0. - 0. - 0. - 0.84 - - 0 - 0 - 0 - - - - - - - -0.15 - 0 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0.15 - 0 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0. - 0.15 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0. - -0.15 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - - - - - - - - - fcs/front_motor - fcs/back_motor - fcs/right_motor - fcs/left_motor - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - 0 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - 0 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/right_motor - 0.84 - - - - 0 - 6.89 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/left_motor - 0.84 - - - - 0 - -6.89 - 0 - - - 0 - 0 - -1 - - - - - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - - 1.9685 - 0 - - - 1 - 0 - 0 - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - -1.9685 - 0 - - - -1 - 0 - 0 - - - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - 1.9685 - 0 - - - 1 - 0 - 0 - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - -1.9685 - 0 - - - -1 - 0 - 0 - - - - - - - - - fcs/right_motor - 0.84 - - - - -1.9685 - 6.89 - 0 - - - 0 - 1 - 0 - - - - - - - fcs/right_motor - 0.84 - - - - 1.9685 - 6.89 - 0 - - - 0 - -1 - 0 - - - - - - - - - fcs/left_motor - 0.84 - - - - -1.9685 - -6.89 - 0 - - - 0 - 1 - 0 - - - - - - - fcs/left_motor - 0.84 - - - - 1.9685 - -6.89 - 0 - - - 0 - -1 - 0 - - - - - - - - - - - - - Drag - - aero/qbar-psf - 47.9 - 0.0151 - 0.224808943 - - - - - - diff --git a/conf/simulator/jsbsim/aircraft/BOOZ2_F1.xml b/conf/simulator/jsbsim/aircraft/BOOZ2_F1.xml deleted file mode 100644 index f55012a4ac..0000000000 --- a/conf/simulator/jsbsim/aircraft/BOOZ2_F1.xml +++ /dev/null @@ -1,396 +0,0 @@ - - - - - - Gustavo Violato & Antoine Drouin - 24-02-2009 - Version 0.9 - beta - Simple Quadrotor without rotor dynamic - - - - 78.53 - 10 - 6.89 - 0 - 0 - 0 - 0 - - 0 - 0 - 0 - - - 0 - 0 - 0 - - - 0 - 0 - 0 - - - - - 0.005 - 0.005 - 0.010 - 0. - 0. - 0. - 0.84 - - 0 - 0 - 0 - - - - - - - -0.15 - 0 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0.15 - 0 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0. - 0.15 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - 0. - -0.15 - -0.1 - - 0.8 - 0.5 - 500 - 100 - 1000 - 0.0 - NONE - 0 - - - - - - - - - - - - - fcs/front_motor - fcs/back_motor - fcs/right_motor - fcs/left_motor - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - 0 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - 0 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/right_motor - 0.84 - - - - 0 - 6.89 - 0 - - - 0 - 0 - -1 - - - - - - - fcs/left_motor - 0.84 - - - - 0 - -6.89 - 0 - - - 0 - 0 - -1 - - - - - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - - 1.9685 - 0 - - - -1 - 0 - 0 - - - - - - - fcs/front_motor - 0.84 - - - - -6.89 - -1.9685 - 0 - - - 1 - 0 - 0 - - - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - 1.9685 - 0 - - - -1 - 0 - 0 - - - - - - - fcs/back_motor - 0.84 - - - - 6.89 - -1.9685 - 0 - - - 1 - 0 - 0 - - - - - - - - - fcs/right_motor - 0.84 - - - - -1.9685 - 6.89 - 0 - - - 0 - -1 - 0 - - - - - - - fcs/right_motor - 0.84 - - - - 1.9685 - 6.89 - 0 - - - 0 - 1 - 0 - - - - - - - - - fcs/left_motor - 0.84 - - - - -1.9685 - -6.89 - 0 - - - 0 - -1 - 0 - - - - - - - fcs/left_motor - 0.84 - - - - 1.9685 - -6.89 - 0 - - - 0 - 1 - 0 - - - - - - - - - - - - - Drag - - aero/qbar-psf - 47.9 - 0.0151 - 0.224808943 - - - - - - diff --git a/conf/simulator/jsbsim/aircraft/Quad_LisaM_2.xml b/conf/simulator/jsbsim/aircraft/simple_quad.xml similarity index 100% rename from conf/simulator/jsbsim/aircraft/Quad_LisaM_2.xml rename to conf/simulator/jsbsim/aircraft/simple_quad.xml diff --git a/conf/simulator/jsbsim/aircraft/fraser.xml b/conf/simulator/jsbsim/aircraft/simple_quad_ccw.xml similarity index 100% rename from conf/simulator/jsbsim/aircraft/fraser.xml rename to conf/simulator/jsbsim/aircraft/simple_quad_ccw.xml diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 335a90ed50..ed6d1110e5 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -67,7 +67,7 @@ void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8 else // 8 data bits by default usart_set_databits((uint32_t)p->reg_addr, 9); } - else if (parity == USART_PARITY_ODD) { + else if (parity == UPARITY_ODD) { usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_ODD); if (bits == UBITS_7) usart_set_databits((uint32_t)p->reg_addr, 8); diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 27aabe4bfe..c337bdf627 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -36,8 +36,31 @@ #include "subsystems/abi.h" #include "led.h" +/** Normal frequency with the default settings + * + * the baro read function should be called at 5 Hz + */ +#ifndef BARO_BOARD_APOGEE_FREQ +#define BARO_BOARD_APOGEE_FREQ 5 +#endif + +/** Baro periodic prescaler + * + * different for fixedwing and rotorcraft... + */ +#ifdef BARO_PERIODIC_FREQUENCY +#define MPL_PRESCALER ((BARO_PERIODIC_FREQUENCY)/BARO_BOARD_APOGEE_FREQ) +#else +#ifdef PERIODIC_FREQUENCY +#define MPL_PRESCALER ((PERIODIC_FREQUENCY)/BARO_BOARD_APOGEE_FREQ) +#else +// default: assuming 60Hz for a 5Hz baro update +#define MPL_PRESCALER 12 +#endif +#endif + /** Counter to init ads1114 at startup */ -#define BARO_STARTUP_COUNTER 200 +#define BARO_STARTUP_COUNTER (200/(MPL_PRESCALER)) uint16_t startup_cnt; struct Mpl3115 apogee_baro; @@ -67,17 +90,15 @@ void baro_periodic( void ) { #endif } // Read the sensor - mpl3115_periodic(&apogee_baro); + RunOnceEvery(MPL_PRESCALER, mpl3115_periodic(&apogee_baro)); } } void apogee_baro_event(void) { mpl3115_event(&apogee_baro); - if (apogee_baro.data_available) { - if (startup_cnt == 0) { - float pressure = ((float)apogee_baro.pressure/(1<<2)); - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); - } + if (apogee_baro.data_available && startup_cnt == 0) { + float pressure = ((float)apogee_baro.pressure/(1<<2)); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); apogee_baro.data_available = FALSE; } } diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c index 3cd631019b..3c181afab6 100644 --- a/sw/airborne/boards/baro_board_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -81,12 +81,12 @@ void baro_periodic(void) { &bb_ms5611.data.c[4], &bb_ms5611.data.c[5], &bb_ms5611.data.c[6], - &bb_ms5611.data.c[7]); + &bb_ms5611.data.c[7])); #endif } } -void baro_event(void){ +void baro_event(void) { if (sys_time.nb_sec > 1) { ms5611_i2c_event(&bb_ms5611); diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 61b0396d78..545bb24375 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -70,12 +70,12 @@ void baro_periodic(void) { &bb_ms5611.data.c[4], &bb_ms5611.data.c[5], &bb_ms5611.data.c[6], - &bb_ms5611.data.c[7]); + &bb_ms5611.data.c[7])); #endif } } -void baro_event(){ +void baro_event(void) { if (sys_time.nb_sec > 1) { ms5611_spi_event(&bb_ms5611); diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h index 49c2fdc26f..87ced73af4 100755 --- a/sw/airborne/boards/stm32f4_discovery.h +++ b/sw/airborne/boards/stm32f4_discovery.h @@ -65,8 +65,8 @@ * PD9 = UART3 RX * PD10 = FREE * PD11 = FREE - * PD12 = LED_3 - * PD13 = LED_4 + * PD12 = LED_4 + * PD13 = LED_3 * PD14 = LED_5 * PD15 = LED_6 * @@ -110,7 +110,7 @@ #endif #define LED_3_GPIO GPIOD #define LED_3_GPIO_CLK RCC_AHB1ENR_IOPDEN -#define LED_3_GPIO_PIN GPIO12 +#define LED_3_GPIO_PIN GPIO13 #define LED_3_AFIO_REMAP ((void)0) #define LED_3_GPIO_ON gpio_set #define LED_3_GPIO_OFF gpio_clear @@ -121,7 +121,7 @@ #endif #define LED_4_GPIO GPIOD #define LED_4_GPIO_CLK RCC_AHB1ENR_IOPDEN -#define LED_4_GPIO_PIN GPIO13 +#define LED_4_GPIO_PIN GPIO12 #define LED_4_AFIO_REMAP ((void)0) #define LED_4_GPIO_ON gpio_set #define LED_4_GPIO_OFF gpio_clear diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 4305be0691..9cdefcb3a2 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -57,6 +57,7 @@ #endif PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF) +PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF) #ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 959cdfb1d8..4f43278139 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -41,6 +41,14 @@ #define GUIDANCE_H_USE_REF TRUE #endif +/** Use horizontal guidance speed reference. + * This also allows to give velocity commands via RC in GUIDANCE_H_MODE_HOVER. + * Default is TRUE, define to FALSE to always disable it. + */ +#ifndef GUIDANCE_H_USE_SPEED_REF +#define GUIDANCE_H_USE_SPEED_REF TRUE +#endif + #define GUIDANCE_H_MODE_KILL 0 #define GUIDANCE_H_MODE_RATE 1 #define GUIDANCE_H_MODE_ATTITUDE 2 diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h index 76f48194e5..0054e5bdfd 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h @@ -46,7 +46,7 @@ struct FloatAttitudeGains { struct FloatVect3 surface_i; }; -extern struct FloatEulers stabilization_att_sum_err_eulers; +extern struct FloatEulers stabilization_att_sum_err; extern float stabilization_att_fb_cmd[COMMANDS_NB]; extern float stabilization_att_ff_cmd[COMMANDS_NB]; 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 d98e976f97..7dadbb42a3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -48,9 +48,9 @@ static void send_att(void) { &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, - &stabilization_att_sum_err_eulers.phi, - &stabilization_att_sum_err_eulers.theta, - &stabilization_att_sum_err_eulers.psi, + &stabilization_att_sum_err.phi, + &stabilization_att_sum_err.theta, + &stabilization_att_sum_err.psi, &stabilization_att_fb_cmd[COMMAND_ROLL], &stabilization_att_fb_cmd[COMMAND_PITCH], &stabilization_att_fb_cmd[COMMAND_YAW], 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 ee7f42f067..232cbbf3c6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -36,7 +36,7 @@ struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]; struct FloatQuat stabilization_att_sum_err_quat; -struct FloatEulers stabilization_att_sum_err_eulers; +struct FloatEulers stabilization_att_sum_err; struct FloatRates last_body_rate; @@ -109,7 +109,7 @@ void stabilization_attitude_init(void) { } FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); - FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); + FLOAT_EULERS_ZERO( stabilization_att_sum_err ); FLOAT_RATES_ZERO( last_body_rate ); } @@ -131,7 +131,7 @@ void stabilization_attitude_enter(void) { stabilization_attitude_ref_enter(); FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); - FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); + FLOAT_EULERS_ZERO( stabilization_att_sum_err ); } void stabilization_attitude_set_failsafe_setpoint(void) { @@ -264,11 +264,11 @@ void stabilization_attitude_run(bool_t enable_integrator) { FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); - FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); + FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); - FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); + FLOAT_EULERS_ZERO( stabilization_att_sum_err ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index 04e3e60b10..a6b0c9d95e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -82,8 +82,8 @@ void stabilization_attitude_ref_update() { stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; /* saturate acceleration */ - const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; - const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \ + const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; + const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); /* saturate speed and trim accel accordingly */ diff --git a/sw/airborne/peripherals/mpl3115.c b/sw/airborne/peripherals/mpl3115.c index bfac2abad1..1ca95a416c 100644 --- a/sw/airborne/peripherals/mpl3115.c +++ b/sw/airborne/peripherals/mpl3115.c @@ -99,7 +99,7 @@ void mpl3115_read(struct Mpl3115 *mpl) mpl->req_trans.buf[0] = MPL3115_REG_CTRL_REG1; mpl->req_trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | (mpl->alt_mode<<7) | MPL3115_OST_BIT); - i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); + i2c_transmit(mpl->i2c_p, &mpl->req_trans, mpl->trans.slave_addr, 2); } } } diff --git a/sw/airborne/state.c b/sw/airborne/state.c index f72bc48438..d2130dd928 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -744,8 +744,9 @@ void stateCalcHorizontalSpeedNorm_i(void) { state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { - /// @todo consider INT32_SPEED_FRAC - INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); + int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + + state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; + INT32_SQRT(state.h_speed_norm_i, n2); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); @@ -753,8 +754,9 @@ void stateCalcHorizontalSpeedNorm_i(void) { state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { - /// @todo consider INT32_SPEED_FRAC - INT32_VECT2_NORM(state.h_speed_norm_i, state.enu_speed_i); + int32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x + + state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC; + INT32_SQRT(state.h_speed_norm_i, n2); } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f); @@ -763,9 +765,11 @@ void stateCalcHorizontalSpeedNorm_i(void) { } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef speed to ned, set status bit, then compute norm */ - //foo - /// @todo consider INT32_SPEED_FRAC - //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); + ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); + SetBit(state.speed_status, SPEED_NED_I); + int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + + state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; + INT32_SQRT(state.h_speed_norm_i, n2); } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c index c6367005c9..59300f23e0 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c @@ -96,10 +96,10 @@ void ahrs_align(void) { static send_gx3(void) { DOWNLINK_SEND_GX3_INFO(DefaultChannel, DefaultDevice, - &ahrs_impl.GX3_freq, - &ahrs_impl.GX3_packet.chksm_error, - &ahrs_impl.GX3_packet.hdr_error, - &ahrs_impl.GX3_chksm); + &ahrs_impl.gx3_freq, + &ahrs_impl.gx3_packet.chksm_error, + &ahrs_impl.gx3_packet.hdr_error, + &ahrs_impl.gx3_chksm); } #endif diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index cf451b7e43..7e7ec3b990 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -140,6 +140,9 @@ void ins_update_gps(void) { #if !USE_BAROMETER float falt = gps.hmsl / 1000.; EstimatorSetAlt(falt); + if (!alt_kalman_enabled) { + ins_alt_dot = -gps.ned_vel.z / 100.; + } #endif utm.alt = ins_alt; // set position diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 6d0c7826d7..7600115d95 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -707,7 +707,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id strip_connect (fun x -> dl_setting_callback id x) with Not_found -> if warning then - fprintf stderr "Warning: %s not setable from GCS strip (i.e. not listed in the xml settings file)" setting_name in + fprintf stderr "Warning: %s not setable from GCS strip (i.e. not listed in the xml settings file)\n" setting_name in connect "flight_altitude" (fun f -> ac.strip#connect_shift_alt (fun x -> f (ac.target_alt+.x))); connect "launch" ac.strip#connect_launch; diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml index 5021a4c450..9468fba30b 100644 --- a/sw/ground_segment/cockpit/page_settings.ml +++ b/sw/ground_segment/cockpit/page_settings.ml @@ -222,7 +222,7 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin let same_tag_for_all = function -[] -> failwith "Page_settings: unreachable, empty dl_settings element" + [] -> failwith "Page_settings: unreachable, empty dl_settings element" | x::xs -> let tag_first = Xml.tag x in List.iter (fun y -> assert(ExtXml.tag_is y tag_first)) xs; diff --git a/sw/ground_segment/python/settings_app/settingsapp.py b/sw/ground_segment/python/settings_app/settingsapp.py index d32fa699ee..9f7eae3080 100755 --- a/sw/ground_segment/python/settings_app/settingsapp.py +++ b/sw/ground_segment/python/settings_app/settingsapp.py @@ -9,7 +9,6 @@ import sys sys.path.append(os.getenv("PAPARAZZI_HOME") + "/sw/lib/python") import settings_xml_parse -DEFAULT_AC_IDS = [ 11 ] # biwhirly def Usage(scmd): lpathitem = scmd.split('/') @@ -40,11 +39,10 @@ def GetOptions(): class SettingsApp(wx.App): def OnInit(self): options = GetOptions() - if len(options['ac_id']) > 0: - ac_ids = options['ac_id'] - else: - ac_ids = DEFAULT_AC_IDS - self.main = settingsframe.create(None, ac_ids) + if not options['ac_id']: + Usage(sys.argv[0]) + sys.exit("Error: Please specify at least one aircraft ID.") + self.main = settingsframe.create(None, options['ac_id']) self.main.Show() self.SetTopWindow(self.main) diff --git a/sw/ground_segment/tmtc/settings.ml b/sw/ground_segment/tmtc/settings.ml index 48afb18dc7..2fb7dd6f83 100644 --- a/sw/ground_segment/tmtc/settings.ml +++ b/sw/ground_segment/tmtc/settings.ml @@ -80,11 +80,20 @@ let _ = let ivy_bus = ref Defivybus.default_ivy_bus in let acs = ref [] in - Arg.parse + + let anon_fun = (fun x -> prerr_endline ("WARNING: don't do anything with "^x)) in + let speclist = [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf " Default is %s" !ivy_bus); "-ac", Arg.String (fun x -> acs := x :: !acs), "A/C name"] - (fun x -> prerr_endline ("WARNING: don't do anything with "^x)) - "Usage: "; + and usage_msg = "Usage: " in + + Arg.parse speclist anon_fun usage_msg; + + if List.length !acs = 0 then begin + prerr_endline "Error: Specify at least one Aircraft for which to display the settings!"; + Arg.usage speclist usage_msg; + exit 1 + end; (** Connect to the Ivy bus *) Ivy.init "Paparazzi settings" "READY" (fun _ _ -> ()); diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index 04d8304d0b..12b3052c4a 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -5,8 +5,22 @@ #include "nps_radio_control.h" +/** + * Number of commands sent to the FDM of NPS. + * If MOTOR_MIXING_NB_MOTOR is defined (usually rotorcraft firmware) + * we have that many commands (one per motor), + * otherwise we default to the number of high level commands (COMMANDS_NB). + */ +#ifndef NPS_COMMANDS_NB +#if defined MOTOR_MIXING_NB_MOTOR +#define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR +#else +#define NPS_COMMANDS_NB COMMANDS_NB +#endif +#endif + struct NpsAutopilot { - double commands[COMMANDS_NB]; + double commands[NPS_COMMANDS_NB]; }; extern struct NpsAutopilot autopilot; diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 409a0c3a43..8f1952e0fd 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -134,7 +134,7 @@ void nps_autopilot_run_step(double time) { Ap(handle_periodic_tasks); /* scale final motor commands to 0-1 for feeding the fdm */ - for (uint8_t i=0; i < COMMANDS_NB; i++) + for (uint8_t i=0; i < NPS_COMMANDS_NB; i++) autopilot.commands[i] = (double)commands[i]/MAX_PPRZ; } diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 681d5bf5d8..ce0b529012 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -52,6 +52,9 @@ bool_t nps_bypass_ins; #define NPS_BYPASS_INS FALSE #endif +#if NPS_COMMANDS_NB != MOTOR_MIXING_NB_MOTOR +#error "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!" +#endif void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) { @@ -114,8 +117,7 @@ void nps_autopilot_run_step(double time) { handle_periodic_tasks(); /* scale final motor commands to 0-1 for feeding the fdm */ - /* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */ - for (uint8_t i=0; i +#include +#include +#include +#include +#include +#include +#include + +#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" + +#ifndef NPS_CRRCSIM_HOST_IP +#define NPS_CRRCSIM_HOST_IP "127.0.0.1" +#endif + +#ifndef NPS_CRRCSIM_HOST_PORT +#define NPS_CRRCSIM_HOST_PORT 9002 +#endif + +#ifndef NPS_CRRCSIM_ROLL_NEUTRAL +#define NPS_CRRCSIM_ROLL_NEUTRAL 0. +#endif + +#ifndef NPS_CRRCSIM_PITCH_NEUTRAL +#define NPS_CRRCSIM_PITCH_NEUTRAL 0. +#endif + +/* blocking */ +#define UDP_BLOCKING 0 +#define UDP_NONBLOCKING 1 + +// type +#define word unsigned short +#define byte unsigned char + +// uNAV packet length definition +#define IMU_PACKET_LENGTH 51 +#define GPS_PACKET_LENGTH 86 +#define AHRS_PACKET_LENGTH 93 +#define FULL_PACKET_SIZE 93 + + +// NpsFdm structure +struct NpsFdm fdm; + +#define INPUT_BUFFER_SIZE (3*FULL_PACKET_SIZE) +// Input buffer +struct inputbuf { + byte buf[INPUT_BUFFER_SIZE]; + byte start; + int length; +}; + +// Socket structure +struct _crrcsim { + int socket; + struct sockaddr_in addr; + struct inputbuf buf; + byte data_buffer[FULL_PACKET_SIZE]; +}; + +static struct _crrcsim crrcsim; + +// Reference point +static struct LtpDef_d ltpdef; + +// static functions declaration +static void open_udp(char* host, int port, int blocking); +static void inputbuf_init(struct inputbuf* c); +static void read_into_buffer(struct _crrcsim* io); +static void init_ltp(void); +static int get_msg(struct _crrcsim* io, byte* data_buffer); +static void decode_imupacket(struct NpsFdm * fdm, byte* buffer); +static void decode_gpspacket(struct NpsFdm * fdm, byte* buffer); +static void decode_ahrspacket(struct NpsFdm * fdm, byte* buffer); +static void send_servo_cmd(struct _crrcsim* io, double* commands); + +// NPS FDM interface +void nps_fdm_init(double dt) { + fdm.init_dt = dt; + fdm.curr_dt = dt; + fdm.nan_count = 0; + + init_ltp(); + + // init circfuf + inputbuf_init(&crrcsim.buf); + + printf("Starting to connect to CRRCsim server.\n"); + open_udp((char*)(NPS_CRRCSIM_HOST_IP), NPS_CRRCSIM_HOST_PORT, UDP_NONBLOCKING); + + if (crrcsim.socket < 0) { + printf("Connection to CRRCsim failed\n"); + exit(0); + } + else { + printf("Connection to CRRCsim succed\n"); + } + + // Send something to let crrcsim that we are here + double zero[] = { 0., 0., 0. }; + send_servo_cmd(&crrcsim, zero); +} + +void nps_fdm_run_step(double* commands, int commands_nb) { + // read state + if (get_msg(&crrcsim, crrcsim.data_buffer) <= 0) { + return; // nothing on the socket + } + + // send commands + send_servo_cmd(&crrcsim, commands); + + //printf("new data %c %c\n", crrcsim.data_buffer[2], crrcsim.data_buffer[33]); + switch (crrcsim.data_buffer[2]) + { + case 'S': /* IMU packet without GPS */ + decode_imupacket(&fdm, crrcsim.data_buffer); + break; + + case 'N': /* IMU packet with GPS */ + + decode_imupacket(&fdm, crrcsim.data_buffer); + + /****************************************** + *check GPS data packet + ******************************************/ + //if(data_buffer[31]==(byte)0x55 && data_buffer[32]==(byte)0x55 && data_buffer[33]=='G') + if(crrcsim.data_buffer[33]=='G') { + decode_gpspacket(&fdm, &crrcsim.data_buffer[31]); + } + break; + + case 'I': /* IMU packet with GPS and AHRS */ + + decode_imupacket(&fdm, crrcsim.data_buffer); + + /****************************************** + *check GPS data packet + ******************************************/ + if(crrcsim.data_buffer[33]=='G') { + decode_gpspacket(&fdm, &crrcsim.data_buffer[31]); + } + if(crrcsim.data_buffer[66]=='A') { + decode_ahrspacket(&fdm, &crrcsim.data_buffer[66]); + } + break; + + default : + printf("invalid data packet...!\n"); + } /* end case */ + +} + +void nps_fdm_set_wind(double speed __attribute__((unused)), double dir __attribute__((unused)), int turbulence_severity __attribute__((unused))) { +} + +/*************************************************************************** + ** Open and configure UDP connection + ****************************************************************************/ + +static void open_udp(char* host, int port, int blocking) +{ + int flags; + + bzero((char *) &crrcsim.addr, sizeof(crrcsim.addr)); + crrcsim.addr.sin_family = AF_INET; + crrcsim.addr.sin_addr.s_addr = inet_addr(host); + crrcsim.addr.sin_port = htons(port); + crrcsim.socket = socket(AF_INET, SOCK_DGRAM, 0); + + //make a nonblocking connection + flags = fcntl(crrcsim.socket, F_GETFL, 0); + fcntl(crrcsim.socket, F_SETFL, flags | O_NONBLOCK); + + if (connect(crrcsim.socket,(struct sockaddr*)&crrcsim.addr,sizeof(crrcsim.addr)) < 0) { + close(crrcsim.socket); + crrcsim.socket = -1; + } + + if(crrcsim.socket != -1 && blocking == UDP_BLOCKING) + { + //restore + fcntl(crrcsim.socket, F_SETFL, flags); + } +} + +static void inputbuf_init(struct inputbuf* c) +{ + c->start = 0; + c->length = 0; +} + +static void read_into_buffer(struct _crrcsim* io) +{ + struct inputbuf *c = &io->buf; + int res; + + if (io->socket >= 0) { + // get latest data from the buffer (crapy but working) + while ((res = recv(io->socket, c->buf, INPUT_BUFFER_SIZE, 0)) > 0) { + c->start = 0; + c->length = res; + } + } +} + +static void init_ltp(void) { + + struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = RadOfDeg((double)NAV_LAT0/1e7); + llh_nav0.lon = RadOfDeg((double)NAV_LON0/1e7); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = (NAV_ALT0 + NAV_MSL0)/1000.; + + struct EcefCoor_d ecef_nav0; + ecef_of_lla_d(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_d(<pdef, &ecef_nav0); + + fdm.ltp_g.x = 0.; + fdm.ltp_g.y = 0.; + fdm.ltp_g.z = 0.; // accel data are already with the correct format + +#ifdef AHRS_H_X +#pragma message "Using magnetic field as defined in airframe file." + fdm.ltp_h.x = AHRS_H_X; + fdm.ltp_h.y = AHRS_H_Y; + fdm.ltp_h.z = AHRS_H_Z; +#else + fdm.ltp_h.x = 0.4912; + fdm.ltp_h.y = 0.1225; + fdm.ltp_h.z = 0.8624; +#endif + +} + +static int get_msg(struct _crrcsim* io, byte* data_buffer) +{ + struct inputbuf *c = &io->buf; + int count = 0; + int packet_len; + int i; + + read_into_buffer(io); + + while (1) { + /********************************************************************* + * Find start of packet: the header (2 bytes) starts with 0x5555 + *********************************************************************/ + while(c->length >= 4 && (c->buf[c->start] != (byte)0x55 || c->buf[(byte)(c->start + 1)] != (byte)0x55)) { + c->start++; + c->length--; + } + if(c->length < 4) + return count; + + /********************************************************************* + * Read packet contents + *********************************************************************/ + packet_len = 0; + switch (c->buf[(byte)(c->start + 2)]) + { + case 'S': + packet_len = IMU_PACKET_LENGTH; + break; + + case 'N': + packet_len = GPS_PACKET_LENGTH; + break; + + case 'I': + packet_len = AHRS_PACKET_LENGTH; + break; + + default: + break; + } + + if(packet_len > 0 && c->length < packet_len) + return count; // not enough data + if(packet_len > 0) { + byte ib; + word rcvchecksum = 0; + word sum = 0; + + for(i = 2, ib = c->start + (byte)2; i < packet_len - 2; i++, ib++) + sum += c->buf[ib]; + rcvchecksum = c->buf[ib++] << 8; + rcvchecksum = rcvchecksum | c->buf[ib++]; + + if(rcvchecksum != sum) { + packet_len = 0; + printf("checksum error\n"); + } + } + // fill data buffer or go to next bytes + if(packet_len > 0) { + for(i = 0; i < packet_len; i++) { + data_buffer[i] = c->buf[c->start]; + c->start++; + c->length--; + } + count++; + } + else { + c->start += 3; + c->length -= 3; + } + } + + return count; +} + +// Long (32bits) in buffer are little endian in gps message +#define LongOfBuf(_buf,_idx) (int32_t)(((uint32_t)_buf[_idx+3]<<24)|((uint32_t)_buf[_idx+2]<<16)|((uint32_t)_buf[_idx+1]<<8)|((uint32_t)_buf[_idx])) +// Unsigned short (16bits) in buffer are little endian in gps message +#define UShortOfBuf(_buf,_idx) (uint16_t)(((uint16_t)_buf[_idx+1]<<8)|((uint16_t)_buf[_idx])) +// Short (16bits) in buffer are big endian in other messages +#define ShortOfBuf(_buf,_idx) (int16_t)(((uint16_t)_buf[_idx]<<8)|((uint16_t)_buf[_idx+1])) + +/*************************************************************************************** + *decode the gps data packet + ***************************************************************************************/ +static void decode_gpspacket(struct NpsFdm * fdm, byte* buffer) +{ + /* gps velocity (1e2 m/s to m/s */ + struct NedCoor_d vel; + vel.x = (double)LongOfBuf(buffer,3)*1.0e-2; + vel.y = (double)LongOfBuf(buffer,7)*1.0e-2; + vel.z = (double)LongOfBuf(buffer,11)*1.0e-2; + fdm->ltp_ecef_vel = vel; + ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, <pdef, &vel); + + /* gps position (1e7 deg to rad and 1e3 m to m) */ + struct LlaCoor_d pos; + pos.lon=(double)LongOfBuf(buffer,15)*1.74533e-9; + pos.lat=(double)LongOfBuf(buffer,19)*1.74533e-9; + pos.alt=(double)LongOfBuf(buffer,23)*1.0e-3; + + pos.lat += ltpdef.lla.lat; + pos.lon += ltpdef.lla.lon; + pos.alt += ltpdef.lla.alt; + + fdm->lla_pos = pos; + ecef_of_lla_d(&fdm->ecef_pos, &pos); + fdm->hmsl = pos.alt - NAV_MSL0/1000.; + + /* gps time */ + fdm->time = (double)UShortOfBuf(buffer,27); + + /* in LTP pprz */ + ned_of_ecef_point_d(&fdm->ltpprz_pos, <pdef, &fdm->ecef_pos); + fdm->lla_pos_pprz = pos; + ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, <pdef, &fdm->ecef_ecef_vel); + +#if NPS_CRRCSIM_DEBUG + printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n", + 57.3*fdm->lla_pos.lat, + 57.3*fdm->lla_pos.lon, + fdm->lla_pos.alt, + fdm->ltp_ecef_vel.x, + fdm->ltp_ecef_vel.y, + fdm->ltp_ecef_vel.z, + fdm->time); +#endif +} + +/*************************************************************************************** + *decode the ahrs data packet + ***************************************************************************************/ +static void decode_ahrspacket(struct NpsFdm * fdm, byte* buffer) +{ + /* euler angles (0.9387340515702713e04 rad to rad) */ + fdm->ltp_to_body_eulers.phi = (double)ShortOfBuf(buffer,1)*0.000106526 - NPS_CRRCSIM_ROLL_NEUTRAL; + fdm->ltp_to_body_eulers.theta = (double)ShortOfBuf(buffer,3)*0.000106526 - NPS_CRRCSIM_PITCH_NEUTRAL; + fdm->ltp_to_body_eulers.psi = (double)ShortOfBuf(buffer,5)*0.000106526; + DOUBLE_QUAT_OF_EULERS(fdm->ltp_to_body_quat, fdm->ltp_to_body_eulers); + +#if NPS_CRRCSIM_DEBUG + printf("decode ahrs %f %f %f\n", + fdm->ltp_to_body_eulers.phi*57.3, + fdm->ltp_to_body_eulers.theta*57.3, + fdm->ltp_to_body_eulers.psi*57.3); +#endif +} + +/*************************************************************************************** + *decode the imu data packet + ***************************************************************************************/ +void decode_imupacket(struct NpsFdm * fdm, byte* buffer) +{ + /* acceleration (0.1670132517315938e04 m/s^2 to m/s^2) */ + fdm->body_ecef_accel.x = (double)ShortOfBuf(buffer,3)*5.98755e-04; + fdm->body_ecef_accel.y = (double)ShortOfBuf(buffer,5)*5.98755e-04; + fdm->body_ecef_accel.z = (double)ShortOfBuf(buffer,7)*5.98755e-04; + + + /* angular rate (0.9387340515702713e4 rad/s to rad/s) */ + fdm->body_ecef_rotvel.p = (double)ShortOfBuf(buffer,9)*1.06526e-04; + fdm->body_ecef_rotvel.q = (double)ShortOfBuf(buffer,11)*1.06526e-04; + fdm->body_ecef_rotvel.r = (double)ShortOfBuf(buffer,13)*1.06526e-04; + + /* magnetic field in Gauss */ + //fdm->mag.x = (double)ShortOfBuf(buffer,15)*6.10352e-05; + //fdm->mag.y = (double)ShortOfBuf(buffer,17)*6.10352e-05; + //fdm->mag.z = (double)ShortOfBuf(buffer,19)*6.10352e-05; + + /* pressure in m and m/s */ + //data->Ps = (double)ShortOfBuf(buffer,27)*3.05176e-01; + //data->Pt = (double)ShortOfBuf(buffer,29)*2.44141e-03; + +#if NPS_CRRCSIM_DEBUG + printf("decode imu | accel %f %f %f | gyro %f %f %f\n", + fdm->body_ecef_accel.x, + fdm->body_ecef_accel.y, + fdm->body_ecef_accel.z, + fdm->body_ecef_rotvel.p, + fdm->body_ecef_rotvel.q, + fdm->body_ecef_rotvel.r); +#endif +} + +// compatibility with OSX +#ifdef __APPLE__ +#define MSG_NOSIGNAL SO_NOSIGPIPE +#endif + +/*************************************************************************************** + * send servo command over udp + ***************************************************************************************/ +static void send_servo_cmd(struct _crrcsim* io, double* commands) +{ + //cnt_cmd[1] = ch1:elevator, cnt_cmd[0] = ch0:aileron, cnt_cmd[2] = ch2:throttle + word cnt_cmd[3]; + byte data[24]={0,}; + short i = 0; + word sum=0; + + word roll = (word)((65535/4)*commands[NPS_CRRCSIM_COMMAND_ROLL] + (65536/2)); + word pitch = (word)((65535/4)*commands[NPS_CRRCSIM_COMMAND_PITCH] + (65536/2)); + + cnt_cmd[0] = roll; + cnt_cmd[1] = -pitch; + cnt_cmd[2] = (word)(65535*commands[NPS_CRRCSIM_COMMAND_THROTTLE]); + +#if NPS_CRRCSIM_DEBUG + printf("send servo %f %f %f | %d %d %d | %d %d\n", + commands[0], + commands[1], + commands[2], + cnt_cmd[0], + cnt_cmd[1], + cnt_cmd[2], + roll, pitch); +#endif + + data[0] = 0x55; + data[1] = 0x55; + data[2] = 0x53; + data[3] = 0x53; + + //aileron ch#0,elevator ch#1,throttle ch#2 + //aileron + data[4] = (byte)(cnt_cmd[0] >> 8); + data[5] = (byte)cnt_cmd[0]; + //elevator + data[6] = (byte)(cnt_cmd[1] >> 8); + data[7] = (byte)cnt_cmd[1]; + //throttle + data[8] = (byte)(cnt_cmd[2] >> 8); + data[9] = (byte)cnt_cmd[2]; + + //checksum + sum = 0xa6; //0x53+0x53 + for(i=4;i<22;i++) sum += data[i]; + + data[22] = (byte)(sum >> 8); + data[23] = (byte)sum; + + //sendout the command packet + if (io->socket >= 0) { + send(io->socket, (char*)data, 24, MSG_NOSIGNAL); + } +} + diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c index 30a438d7e2..7102706eab 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.c +++ b/sw/simulator/nps/nps_fdm_jsbsim.c @@ -71,7 +71,7 @@ using namespace JSBSim; using namespace std; -static void feed_jsbsim(double* commands); +static void feed_jsbsim(double* commands, int commands_nb); static void fetch_state(void); static int check_for_nan(void); @@ -124,9 +124,9 @@ void nps_fdm_init(double dt) { } -void nps_fdm_run_step(double* commands) { +void nps_fdm_run_step(double* commands, int commands_nb) { - feed_jsbsim(commands); + feed_jsbsim(commands, commands_nb); /* To deal with ground interaction issues, we decrease the time step as the vehicle is close to the ground. This is done predictively @@ -193,16 +193,17 @@ void nps_fdm_set_wind(double speed, double dir, int turbulence_severity) { /** * Feed JSBSim with the latest actuator commands. * - * @param commands Pointer to array of doubles holding actuator commands + * @param commands Pointer to array of doubles holding actuator commands + * @param commands_nb Number of commands (length of array) */ -static void feed_jsbsim(double* commands) { +static void feed_jsbsim(double* commands, int commands_nb) { char buf[64]; const char* names[] = NPS_ACTUATOR_NAMES; string property; int i; - for (i=0; iGetPropertyManager()->SetDouble(property,commands[i]); diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index 6686beb324..fd1475b97c 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -158,7 +158,7 @@ static void nps_main_run_sim_step(void) { nps_autopilot_run_systime_step(); - nps_fdm_run_step(autopilot.commands); + nps_fdm_run_step(autopilot.commands, NPS_COMMANDS_NB); nps_sensors_run_step(nps_main.sim_time); diff --git a/sw/tools/gen_periodic.ml b/sw/tools/gen_periodic.ml index af18793702..ff7a2fa0eb 100644 --- a/sw/tools/gen_periodic.ml +++ b/sw/tools/gen_periodic.ml @@ -98,31 +98,35 @@ let output_modes = fun out_h process_name modes freq modules -> modes let write_settings = fun xml_file out_set telemetry_xml -> + (* filter xml file to remove unneeded process and modes (more than 1 mode per process) *) + let filtered_xml = List.filter (fun p -> List.length (Xml.children p) > 1) (Xml.children telemetry_xml) in fprintf out_set "\n" xml_file; fprintf out_set "\n\n"; fprintf out_set "\n"; - fprintf out_set " \n"; - fprintf out_set " \n"; - List.iter (fun p -> - (* for each process *) - let process_name = Xml.attrib p "name" in - (* convert the xml list of mode to a string list *) - let modes = List.map (fun m -> Xml.attrib m "name") (Xml.children p) in - let nb_modes = List.length modes in - match nb_modes with - 0 | 1 -> () (* Nothing to do if 1 or zero mode *) + if List.length filtered_xml > 0 then begin + fprintf out_set " \n"; + fprintf out_set " \n"; + List.iter (fun p -> + (* for each (pre-filtered) process *) + let process_name = Xml.attrib p "name" in + (* convert the xml list of mode to a string list *) + let modes = List.map (fun m -> Xml.attrib m "name") (Xml.children p) in + let nb_modes = List.length modes in + match nb_modes with + | 0 | 1 -> () (* Nothing to do if 1 or zero mode *) | _ -> (* add settings with all modes *) - fprintf out_set " \n" (nb_modes-1) process_name process_name (String.concat "|" modes); - let i = ref 0 in - List.iter (fun m -> try - let key = Xml.attrib m "key_press" in - fprintf out_set " \n" key (string_of_int !i); - incr i + fprintf out_set " \n" (nb_modes-1) process_name process_name (String.concat "|" modes); + let i = ref 0 in + List.iter (fun m -> try + let key = Xml.attrib m "key_press" in + fprintf out_set " \n" key (string_of_int !i); + incr i with _ -> incr i) (Xml.children p); - fprintf out_set " \n" - ) (Xml.children telemetry_xml); - fprintf out_set " \n"; - fprintf out_set " \n"; + fprintf out_set " \n" + ) filtered_xml; + fprintf out_set " \n"; + fprintf out_set " \n"; + end; fprintf out_set "\n"