diff --git a/conf/Makefile.ardrone2 b/conf/Makefile.ardrone2
index fc5f0bb565..ad6e9f254b 100644
--- a/conf/Makefile.ardrone2
+++ b/conf/Makefile.ardrone2
@@ -33,9 +33,9 @@ upload_extra:
# Program the device and start it.
load upload program: upload_extra $(OBJDIR)/$(TARGET).elf
- $(Q)$(DRONE) insmod $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko
- $(Q)$(DRONE) upload_paparazzi $(OBJDIR)/$(TARGET).elf $(SUB_DIR)
- $(Q)$(DRONE) status
+ $(Q)$(DRONE) --host=$(HOST) insmod $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko
+ $(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR)
+ $(Q)$(DRONE) --host=$(HOST) status
# Program the device and start it.
diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd
index a8ce45cd30..493ba98742 100644
--- a/conf/airframes/airframe.dtd
+++ b/conf/airframes/airframe.dtd
@@ -121,4 +121,5 @@ target CDATA #IMPLIED>
+target CDATA #IMPLIED
+dir CDATA #IMPLIED>
diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml
index 265e887ffd..3452e08df1 100644
--- a/conf/airframes/examples/quadrotor_lisa_mx.xml
+++ b/conf/airframes/examples/quadrotor_lisa_mx.xml
@@ -41,6 +41,7 @@
+
@@ -50,6 +51,8 @@
+
+
diff --git a/conf/airframes/examples/quadrotor_navstik.xml b/conf/airframes/examples/quadrotor_navstik.xml
new file mode 100644
index 0000000000..06329a360e
--- /dev/null
+++ b/conf/airframes/examples/quadrotor_navstik.xml
@@ -0,0 +1,212 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/rotorcraft_autopilot.xml b/conf/autopilot/rotorcraft_autopilot.xml
index fe8bdbc30b..807e08f721 100644
--- a/conf/autopilot/rotorcraft_autopilot.xml
+++ b/conf/autopilot/rotorcraft_autopilot.xml
@@ -1,17 +1,22 @@
-
+
+
+
+
+
+
-
+
-
-
+
+
@@ -19,28 +24,28 @@
-
+
-
+
-
+
-
+
-
+
@@ -51,15 +56,14 @@
-
+
-
-
+
-
+
@@ -67,20 +71,20 @@
-
-
+
+
-
+
-
-
-
-
+
+
+
+
diff --git a/conf/boards/navstik_1.0.makefile b/conf/boards/navstik_1.0.makefile
new file mode 100644
index 0000000000..a8ab68ce48
--- /dev/null
+++ b/conf/boards/navstik_1.0.makefile
@@ -0,0 +1,54 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# navstik_1.0.makefile
+#
+# http://paparazzi.enac.fr/wiki/Navstik
+#
+
+BOARD=navstik
+BOARD_VERSION=1.0
+BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
+
+ARCH=stm32
+ARCH_L=f4
+HARD_FLOAT=yes
+$(TARGET).ARCHDIR = $(ARCH)
+$(TARGET).OOCD_INTERFACE=ftdi/ivygs
+$(TARGET).OOCD_BOARD=navstik
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/navstik.ld
+
+# -----------------------------------------------------------------------
+
+# default flash mode is via usb dfu bootloader
+# other possibilities: DFU-UTIL, JTAG
+FLASH_MODE ?= JTAG
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= 2
+GPS_LED ?= none
+SYS_TIME_LED ?= 1
+
+#
+# default uart configuration
+#
+RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART6
+
+MODEM_PORT ?= UART5
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART2
+GPS_BAUD ?= B57600
+
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g.
+#
+ACTUATORS ?= actuators_pwm
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index 532ee1d205..c758de27b4 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -56,7 +56,7 @@
radio="radios/cockpitMM.xml"
telemetry="telemetry/default_fixedwing.xml"
flight_plan="flight_plans/basic.xml"
- settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/infrared.xml"
+ settings="settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/infrared.xml"
gui_color="#6293ba"
/>
+
+
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+IMU_CFLAGS = -DUSE_IMU -DIMU_NAVSTIK -DIMU_TYPE_H=\"imu/imu_navstik.h\"
+IMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_navstik.c
+IMU_SRCS += peripherals/hmc58xx.c
+IMU_SRCS += peripherals/mpu60x0.c peripherals/mpu60x0_i2c.c
+
+
+NAVSTIK_MAG_I2C_DEV ?= i2c3
+NAVSTIK_MPU_I2C_DEV ?= i2c1
+
+NAVSTIK_MAG_I2C_DEV_UPPER=$(shell echo $(NAVSTIK_MAG_I2C_DEV) | tr a-z A-Z)
+NAVSTIK_MAG_I2C_DEV_LOWER=$(shell echo $(NAVSTIK_MAG_I2C_DEV) | tr A-Z a-z)
+NAVSTIK_MPU_I2C_DEV_UPPER=$(shell echo $(NAVSTIK_MPU_I2C_DEV) | tr a-z A-Z)
+NAVSTIK_MPU_I2C_DEV_LOWER=$(shell echo $(NAVSTIK_MPU_I2C_DEV) | tr A-Z a-z)
+
+IMU_CFLAGS += -DNAVSTIK_MAG_I2C_DEV=$(NAVSTIK_MAG_I2C_DEV_LOWER) -DNAVSTIK_MPU_I2C_DEV=$(NAVSTIK_MPU_I2C_DEV_LOWER)
+IMU_CFLAGS += -DUSE_$(NAVSTIK_MAG_I2C_DEV_UPPER)=1 -DUSE_$(NAVSTIK_MPU_I2C_DEV_UPPER)=1
+
+ap.CFLAGS += $(IMU_CFLAGS)
+ap.srcs += $(IMU_SRCS)
+
+test_imu.CFLAGS += $(IMU_CFLAGS)
+test_imu.srcs += $(IMU_SRCS)
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile
index a98d2d2ab0..68d1003b29 100644
--- a/conf/firmwares/test_progs.makefile
+++ b/conf/firmwares/test_progs.makefile
@@ -86,6 +86,9 @@ ifeq ($(BOARD_VERSION), 2.0)
LED_DEFINES = -DLED_BLUE=3 -DLED_RED=4 -DLED_GREEN=5
endif
endif
+ifeq ($(BOARD), navstik)
+LED_DEFINES = -DLED_RED=1 -DLED_GREEN=2
+endif
LED_DEFINES ?= -DLED_RED=2 -DLED_GREEN=3
test_sys_time_timer.ARCHDIR = $(ARCH)
diff --git a/conf/flash_modes.xml b/conf/flash_modes.xml
index f7ebd9bda1..614bc97b39 100644
--- a/conf/flash_modes.xml
+++ b/conf/flash_modes.xml
@@ -53,6 +53,7 @@
+
@@ -88,6 +89,7 @@
+
diff --git a/conf/flight_plans/cube.xml b/conf/flight_plans/cube.xml
index aa4487f645..94c1845896 100644
--- a/conf/flight_plans/cube.xml
+++ b/conf/flight_plans/cube.xml
@@ -33,26 +33,26 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 2bd2bc9df2..f3fb42050a 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -215,7 +215,7 @@
-
+
@@ -916,7 +916,7 @@
-
+
@@ -996,7 +996,7 @@
-
+
@@ -1005,7 +1005,7 @@
-
+
@@ -1973,7 +1973,7 @@
-
+
diff --git a/conf/messages_ng.xml b/conf/messages_ng.xml
new file mode 100644
index 0000000000..16f8914974
--- /dev/null
+++ b/conf/messages_ng.xml
@@ -0,0 +1,2800 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/mf_ptu.xml b/conf/modules/mf_ptu.xml
new file mode 100644
index 0000000000..3d87087405
--- /dev/null
+++ b/conf/modules/mf_ptu.xml
@@ -0,0 +1,50 @@
+
+
+
+
+
+ PTU board from Meteo France (pressure, temperature, humidity).
+
+ Raw meteo data:
+ - Pressure in ADC
+ - Temperature in ADC
+ - Humidity in micro-seconds (period of the input signal)
+
+ When using SEND_PTU flag, scaled pressure, temperature and humidity data are sent over telemetry with the PAYLOAD_FLOAT message (array of float).
+ If scale factors and offset are not specified, raw values are sent (scale=1.0, offset=0).
+
+ When using LOG_PTU flag, raw pressure, temperature and humidity data are stored with raw gps data (position, speed, time, status).
+ Field names and units are in the first line of the log file.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/module.dtd b/conf/modules/module.dtd
index a7ef5e7c7f..c40c761964 100644
--- a/conf/modules/module.dtd
+++ b/conf/modules/module.dtd
@@ -9,11 +9,12 @@
-
+
+
@@ -69,6 +70,10 @@ unit CDATA #IMPLIED
type CDATA #IMPLIED
description CDATA #IMPLIED>
+
+
diff --git a/conf/modules/pwm_meas.xml b/conf/modules/pwm_meas.xml
index b92612b581..2430ea6101 100644
--- a/conf/modules/pwm_meas.xml
+++ b/conf/modules/pwm_meas.xml
@@ -11,8 +11,12 @@
- INPUT CAPTURE CAP0.3 on P0.29 (TWOG ADC5, 5V to 3.3V voltage divider)
- INPUT CAPTURE CAP0.0 on P0.30 (TWOG ADC4, no voltage divider)
- Currently only available on LPC21xx arch
+ For STM32:
+ - each board (or airframe file) has to define the PWM input pin and parameters
+ - example can be found in sw/airborne/boards/apogee_1.0.h board file
+
+
diff --git a/conf/settings/estimation/ins_float_invariant.xml b/conf/settings/estimation/ins_float_invariant.xml
index e7958d7a18..30ac06cd03 100644
--- a/conf/settings/estimation/ins_float_invariant.xml
+++ b/conf/settings/estimation/ins_float_invariant.xml
@@ -3,6 +3,7 @@
+
diff --git a/conf/system/udev/rules/50-paparazzi.rules b/conf/system/udev/rules/50-paparazzi.rules
index 6d50b0b908..50c04ab97a 100644
--- a/conf/system/udev/rules/50-paparazzi.rules
+++ b/conf/system/udev/rules/50-paparazzi.rules
@@ -9,6 +9,10 @@ SUBSYSTEM=="tty", ATTRS{product}=="FT232R USB UART", SYMLINK+="paparazzi/serial"
# MaxStream xbee pro box
SUBSYSTEM=="tty", ATTRS{product}=="MaxStream PKG-U", SYMLINK+="paparazzi/xbee", GROUP="plugdev"
+# Navstik Xbee port
+SUBSYSTEMS=="usb", ENV{.LOCAL_ifNum}="$attr{bInterfaceNumber}"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{product}=="Quad RS232-HS", ENV{.LOCAL_ifNum}=="02", SYMLINK+="paparazzi/xbee", GROUP="plugdev"
+
# Recent Digi XBee pro modems (XBP24-PKC-001-UA)
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{product}=="USB <-> Serial", SYMLINK+="paparazzi/xbee", GROUP="plugdev"
@@ -27,6 +31,9 @@ ATTRS{idVendor}=="0403", ATTRS{idProduct}=="cff8", GROUP="plugdev"
# FTDI 2232 based jtag for Lisa/L and usb upload
ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6010", MODE="0666", GROUP="plugdev"
+# FTDI 2232 based jtag for Navstik
+ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", MODE="0666", GROUP="plugdev"
+
# dfu devices
ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE="0666", GROUP="plugdev"
diff --git a/sw/airborne/Makefile b/sw/airborne/Makefile
index c2be06ff87..8005c4f213 100644
--- a/sw/airborne/Makefile
+++ b/sw/airborne/Makefile
@@ -33,6 +33,7 @@ VARINCLUDE = $(PAPARAZZI_HOME)/var/include
INCLUDES = -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE)
+VPATH = .
ifneq ($(MAKECMDGOALS),clean)
include $(AIRCRAFT_BUILD_DIR)/Makefile.ac
diff --git a/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.h b/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.h
index 31e176daf5..108cca5b6c 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.h
+++ b/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.h
@@ -34,7 +34,11 @@
#include "LPC21xx.h"
#include "interrupt_hw.h"
-#define PWM_INPUT_NB 2 //this is architecture dependent
+enum pwm_input_channels {
+ PWM_INPUT1,
+ PWM_INPUT2,
+ PWM_INPUT_NB
+};
#include "mcu_periph/pwm_input.h"
diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c
index 7fcdfab363..717696224f 100644
--- a/sw/airborne/arch/stm32/mcu_arch.c
+++ b/sw/airborne/arch/stm32/mcu_arch.c
@@ -39,6 +39,23 @@
#include "std.h"
+#if defined(STM32F4)
+/** 25MHz external clock to PLL it to 168MHz */
+const clock_scale_t hse_25mhz_3v3_168mhz = { /* 168MHz */
+ .pllm = 25,
+ .plln = 336,
+ .pllp = 2,
+ .pllq = 7,
+ .hpre = RCC_CFGR_HPRE_DIV_NONE,
+ .ppre1 = RCC_CFGR_PPRE_DIV_4,
+ .ppre2 = RCC_CFGR_PPRE_DIV_2,
+ .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE |
+ FLASH_ACR_LATENCY_5WS,
+ .apb1_frequency = 42000000,
+ .apb2_frequency = 84000000,
+};
+#endif
+
void mcu_arch_init(void) {
#if LUFTBOOT
PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.")
@@ -65,6 +82,11 @@ PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 168MHz.")
PRINT_CONFIG_MSG("Using 16MHz external clock to PLL it to 168MHz.")
rcc_clock_setup_hse_3v3(&hse_16mhz_3v3[CLOCK_3V3_168MHZ]);
#endif
+#elif EXT_CLK == 25000000
+#if defined(STM32F4)
+PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.")
+ rcc_clock_setup_hse_3v3(&hse_25mhz_3v3_168mhz);
+#endif
#else
#error EXT_CLK is either set to an unsupported frequency or not defined at all. Please check!
#endif
@@ -118,10 +140,8 @@ uint32_t timer_get_frequency(uint32_t timer_peripheral)
{
switch (timer_peripheral) {
// Timers on APB1
-#if ADVANCED_TIMERS
case TIM1:
case TIM8:
-#endif
#ifdef TIM9
case TIM9:
#endif
diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
index 092acd1afd..f9501944fd 100644
--- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
@@ -97,7 +97,6 @@
#include "mcu_periph/gpio.h"
#include "mcu_arch.h"
#include "std.h"
-#include "led.h"
#include BOARD_CONFIG
@@ -190,8 +189,6 @@ static inline void adc_init_irq( void );
* for the particular adc converter.
*/
-volatile uint8_t adc_new_data_trigger;
-
static uint8_t nb_adc1_channels = 0;
static uint8_t nb_adc2_channels = 0;
static uint8_t nb_adc3_channels = 0;
@@ -350,8 +347,6 @@ void adc_init( void ) {
adc_init_single(ADC3, nb_adc3_channels, adc_channel_map);
#endif // USE_AD3
- adc_new_data_trigger = FALSE;
-
#if USE_ADC_WATCHDOG
adc_watchdog.cb = NULL;
adc_watchdog.timeStamp=0;
@@ -571,7 +566,7 @@ static inline void adc_push_sample(struct adc_buf * buf, uint16_t value) {
#if defined(STM32F1)
void adc1_2_isr(void)
#elif defined(STM32F4)
- void adc_isr(void)
+void adc_isr(void)
#endif
{
uint8_t channel = 0;
@@ -614,12 +609,9 @@ void adc1_2_isr(void)
#if USE_ADC_WATCHDOG
}
#endif
-
-#if !USE_AD2 && !USE_AD3
- adc_new_data_trigger = TRUE;
-#endif
}
-#endif
+#endif // USE_AD1
+
#if USE_AD2
if (adc_eoc_injected(ADC2)){
ADC_SR(ADC2) &= ~ADC_SR_JEOC;
@@ -635,12 +627,10 @@ void adc1_2_isr(void)
}
#if USE_ADC_WATCHDOG
}
-#endif
-#if !USE_AD3
- adc_new_data_trigger = TRUE;
#endif
}
-#endif
+#endif // USE_AD2
+
#if USE_AD3
if (adc_eoc_injected(ADC3)){
ADC_SR(ADC3) &= ~ADC_SR_JEOC;
@@ -657,10 +647,8 @@ void adc1_2_isr(void)
#if USE_ADC_WATCHDOG
}
#endif
- adc_new_data_trigger = TRUE;
}
-#endif
-
+#endif // USE_AD3
return;
}
diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
index 56233fc065..83de90dc74 100644
--- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c
@@ -86,7 +86,7 @@ void gpio_setup_input(uint32_t port, uint16_t gpios) {
gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, gpios);
}
-void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint8_t af, bool_t is_output) {
+void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint32_t af, bool_t is_output) {
gpio_enable_clock(port);
/* remap alternate function if needed */
if (af) {
diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h
index 4129b82390..f72d511e27 100644
--- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h
@@ -51,7 +51,11 @@ extern void gpio_setup_input(uint32_t port, uint16_t gpios);
* Setup a gpio for input or output with alternate function.
* This is an STM32 specific helper funtion and should only be used in stm32 arch code.
*/
+#if defined(STM32F1)
+extern void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint32_t af, bool_t is_output);
+#else
extern void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint8_t af, bool_t is_output);
+#endif
/**
* Setup a gpio for analog use.
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
index 0a3992dda1..72034f4d7a 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
@@ -982,7 +982,7 @@ void i2c1_hw_init(void) {
#else
/* reset peripheral to default state ( sometimes not achieved on reset :( ) */
- //i2c_reset(I2C1);
+ //rcc_periph_reset_pulse(RST_I2C1);
/* Configure and enable I2C1 event interrupt --------------------------------*/
nvic_set_priority(NVIC_I2C1_EV_IRQ, NVIC_I2C1_IRQ_PRIO);
@@ -998,7 +998,7 @@ void i2c1_hw_init(void) {
/* setup gpio clock and pins */
i2c_setup_gpio(I2C1);
- i2c_reset(I2C1);
+ rcc_periph_reset_pulse(RST_I2C1);
// enable peripheral
i2c_peripheral_enable(I2C1);
@@ -1056,7 +1056,7 @@ void i2c2_hw_init(void) {
ZEROS_ERR_COUNTER(i2c2_errors);
/* reset peripheral to default state ( sometimes not achieved on reset :( ) */
- //i2c_reset(I2C2);
+ //rcc_periph_reset_pulse(RST_I2C2);
/* Configure and enable I2C2 event interrupt --------------------------------*/
nvic_set_priority(NVIC_I2C2_EV_IRQ, NVIC_I2C2_IRQ_PRIO);
@@ -1073,7 +1073,7 @@ void i2c2_hw_init(void) {
/* setup gpio clock and pins */
i2c_setup_gpio(I2C2);
- i2c_reset(I2C2);
+ rcc_periph_reset_pulse(RST_I2C2);
// enable peripheral
i2c_peripheral_enable(I2C2);
@@ -1131,7 +1131,7 @@ void i2c3_hw_init(void) {
ZEROS_ERR_COUNTER(i2c3_errors);
/* reset peripheral to default state ( sometimes not achieved on reset :( ) */
- //i2c_reset(I2C3);
+ //rcc_periph_reset_pulse(RST_I2C3);
/* Configure and enable I2C3 event interrupt --------------------------------*/
nvic_set_priority(NVIC_I2C3_EV_IRQ, NVIC_I2C3_IRQ_PRIO);
@@ -1148,7 +1148,7 @@ void i2c3_hw_init(void) {
/* setup gpio clock and pins */
i2c_setup_gpio(I2C3);
- i2c_reset(I2C3);
+ rcc_periph_reset_pulse(RST_I2C3);
// enable peripheral
i2c_peripheral_enable(I2C3);
diff --git a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c
new file mode 100644
index 0000000000..a36a3fe912
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file arch/stm32/mcu_periph/pwm_input_arch.c
+ * @ingroup stm32_arch
+ *
+ * handling of smt32 PWM input using a timer with capture.
+ */
+#include "mcu_periph/pwm_input_arch.h"
+
+#include BOARD_CONFIG
+#include "generated/airframe.h"
+
+#include
+#include
+#include
+#include
+
+#include "mcu_periph/sys_time.h"
+#include "mcu_periph/gpio.h"
+
+// for timer_get_frequency
+#include "mcu_arch.h"
+
+#define ONE_MHZ_CLK 1000000
+#ifdef NVIC_TIM_IRQ_PRIO
+#define PWM_INPUT_IRQ_PRIO NVIC_TIM_IRQ_PRIO
+#else
+#define PWM_INPUT_IRQ_PRIO 2
+#endif
+
+static inline void pwm_input_set_timer(uint32_t tim) {
+ timer_reset(tim);
+ timer_set_mode(tim, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
+ timer_set_period(tim, 0xFFFF);
+ uint32_t timer_clk = timer_get_frequency(tim);
+ timer_set_prescaler(tim, (timer_clk / (PWM_INPUT_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1);
+ timer_enable_counter(tim);
+}
+
+void pwm_input_init ( void )
+{
+ int i;
+ // initialize the arrays to 0
+ for (i = 0; i < PWM_INPUT_NB; i++) {
+ pwm_input_duty_tics[i] = 0;
+ pwm_input_duty_valid[i] = 0;
+ pwm_input_period_tics[i] = 0;
+ pwm_input_period_valid[i] = 0;
+ }
+
+ /** Configure timers
+ * - timer clock enable
+ * - base configuration
+ * - enable counter
+ */
+#if USE_PWM_INPUT_TIM1
+ rcc_periph_clock_enable(RCC_TIM1);
+ pwm_input_set_timer(TIM1);
+#endif
+#if USE_PWM_INPUT_TIM2
+ rcc_periph_clock_enable(RCC_TIM2);
+ pwm_input_set_timer(TIM2);
+#endif
+#if USE_PWM_INPUT_TIM3
+ rcc_periph_clock_enable(RCC_TIM3);
+ pwm_input_set_timer(TIM3);
+#endif
+
+#ifdef USE_PWM_INPUT1
+ /* GPIO configuration as input capture for timer */
+ gpio_setup_pin_af(PWM_INPUT1_GPIO_PORT, PWM_INPUT1_GPIO_PIN, PWM_INPUT1_GPIO_AF, FALSE);
+
+ /** TIM configuration: Input Capture mode
+ * Two IC signals are mapped to the same TI input
+ */
+ timer_ic_set_input(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD, PWM_INPUT1_TIMER_INPUT);
+ timer_ic_set_input(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY, PWM_INPUT1_TIMER_INPUT);
+#if USE_PWM_INPUT1 == PWM_PULSE_TYPE_ACTIVE_LOW
+ timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD, TIM_IC_RISING);
+ timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY, TIM_IC_FALLING);
+#elif USE_PWM_INPUT1 == PWM_PULSE_TYPE_ACTIVE_HIGH
+ timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD, TIM_IC_FALLING);
+ timer_ic_set_polarity(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY, TIM_IC_RISING);
+#endif
+
+ /* Select the valid trigger input */
+ timer_slave_set_trigger(PWM_INPUT1_TIMER, PWM_INPUT1_SLAVE_TRIG);
+ /* Configure the slave mode controller in reset mode */
+ timer_slave_set_mode(PWM_INPUT1_TIMER, TIM_SMCR_SMS_RM);
+
+ /* Enable timer Interrupt(s). */
+ nvic_set_priority(PWM_INPUT1_IRQ, PWM_INPUT_IRQ_PRIO);
+ nvic_enable_irq(PWM_INPUT1_IRQ);
+#ifdef PWM_INPUT1_IRQ2
+ nvic_set_priority(PWM_INPUT1_IRQ2, PWM_INPUT_IRQ_PRIO);
+ nvic_enable_irq(PWM_INPUT1_IRQ2);
+#endif
+
+ /* Enable the Capture/Compare and Update interrupt requests. */
+ timer_enable_irq(PWM_INPUT1_TIMER, (PWM_INPUT1_CC_IE | TIM_DIER_UIE));
+
+ /* Enable capture channel. */
+ timer_ic_enable(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_PERIOD);
+ timer_ic_enable(PWM_INPUT1_TIMER, PWM_INPUT1_CHANNEL_DUTY);
+#endif
+
+#ifdef USE_PWM_INPUT2
+ /* GPIO configuration as input capture for timer */
+ gpio_setup_pin_af(PWM_INPUT2_GPIO_PORT, PWM_INPUT2_GPIO_PIN, PWM_INPUT2_GPIO_AF, FALSE);
+
+ /** TIM configuration: Input Capture mode
+ * Two IC signals are mapped to the same TI input
+ */
+ timer_ic_set_input(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD, PWM_INPUT2_TIMER_INPUT);
+ timer_ic_set_input(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY, PWM_INPUT2_TIMER_INPUT);
+#if USE_PWM_INPUT2 == PWM_PULSE_TYPE_ACTIVE_LOW
+ timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD, TIM_IC_RISING);
+ timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY, TIM_IC_FALLING);
+#elif USE_PWM_INPUT2 == PWM_PULSE_TYPE_ACTIVE_HIGH
+ timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD, TIM_IC_FALLING);
+ timer_ic_set_polarity(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY, TIM_IC_RISING);
+#endif
+
+ /* Select the valid trigger input */
+ timer_slave_set_trigger(PWM_INPUT2_TIMER, PWM_INPUT2_SLAVE_TRIG);
+ /* Configure the slave mode controller in reset mode */
+ timer_slave_set_mode(PWM_INPUT2_TIMER, TIM_SMCR_SMS_RM);
+
+ /* Enable timer Interrupt(s). */
+ nvic_set_priority(PWM_INPUT2_IRQ, PWM_INPUT_IRQ_PRIO);
+ nvic_enable_irq(PWM_INPUT2_IRQ);
+#ifdef PWM_INPUT2_IRQ2
+ nvic_set_priority(PWM_INPUT2_IRQ2, PWM_INPUT_IRQ_PRIO);
+ nvic_enable_irq(PWM_INPUT2_IRQ2);
+#endif
+
+ /* Enable the Capture/Compare and Update interrupt requests. */
+ timer_enable_irq(PWM_INPUT2_TIMER, (PWM_INPUT2_CC_IE | TIM_DIER_UIE));
+
+ /* Enable capture channel. */
+ timer_ic_enable(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_PERIOD);
+ timer_ic_enable(PWM_INPUT2_TIMER, PWM_INPUT2_CHANNEL_DUTY);
+#endif
+
+}
+
+#if USE_PWM_INPUT_TIM1
+
+#if defined(STM32F1)
+void tim1_up_isr(void) {
+#elif defined(STM32F4)
+void tim1_up_tim10_isr(void) {
+#endif
+ if((TIM1_SR & TIM_SR_UIF) != 0) {
+ timer_clear_flag(TIM1, TIM_SR_UIF);
+ // FIXME clear overflow interrupt but what else ?
+ }
+}
+
+void tim1_cc_isr(void) {
+ if((TIM1_SR & TIM1_CC_IF_PERIOD) != 0) {
+ timer_clear_flag(TIM1, TIM1_CC_IF_PERIOD);
+ pwm_input_period_tics[TIM1_PWM_INPUT_IDX] = TIM1_CCR_PERIOD;
+ pwm_input_period_valid[TIM1_PWM_INPUT_IDX] = TRUE;
+ }
+ if((TIM1_SR & TIM1_CC_IF_DUTY) != 0) {
+ timer_clear_flag(TIM1, TIM1_CC_IF_DUTY);
+ pwm_input_duty_tics[TIM1_PWM_INPUT_IDX] = TIM1_CCR_DUTY;
+ pwm_input_duty_valid[TIM1_PWM_INPUT_IDX] = TRUE;
+ }
+}
+
+#endif
+
+#if USE_PWM_INPUT_TIM2
+
+void tim2_isr(void) {
+ if((TIM2_SR & TIM2_CC_IF_PERIOD) != 0) {
+ timer_clear_flag(TIM2, TIM2_CC_IF_PERIOD);
+ pwm_input_period_tics[TIM2_PWM_INPUT_IDX] = TIM2_CCR_PERIOD;
+ pwm_input_period_valid[TIM2_PWM_INPUT_IDX] = TRUE;
+ }
+ if((TIM2_SR & TIM2_CC_IF_DUTY) != 0) {
+ timer_clear_flag(TIM2, TIM2_CC_IF_DUTY);
+ pwm_input_duty_tics[TIM2_PWM_INPUT_IDX] = TIM2_CCR_DUTY;
+ pwm_input_duty_valid[TIM2_PWM_INPUT_IDX] = TRUE;
+ }
+ if((TIM2_SR & TIM_SR_UIF) != 0) {
+ timer_clear_flag(TIM2, TIM_SR_UIF);
+ // FIXME clear overflow interrupt but what else ?
+ }
+}
+
+#endif
+
+#if USE_PWM_INPUT_TIM3
+
+void tim3_isr(void) {
+ if((TIM3_SR & TIM3_CC_IF_PERIOD) != 0) {
+ timer_clear_flag(TIM3, TIM3_CC_IF_PERIOD);
+ pwm_input_period_tics[TIM3_PWM_INPUT_IDX] = TIM3_CCR_PERIOD;
+ pwm_input_period_valid[TIM3_PWM_INPUT_IDX] = TRUE;
+ }
+ if((TIM3_SR & TIM3_CC_IF_DUTY) != 0) {
+ timer_clear_flag(TIM3, TIM3_CC_IF_DUTY);
+ pwm_input_duty_tics[TIM3_PWM_INPUT_IDX] = TIM3_CCR_DUTY;
+ pwm_input_duty_valid[TIM3_PWM_INPUT_IDX] = TRUE;
+ }
+ if((TIM3_SR & TIM_SR_UIF) != 0) {
+ timer_clear_flag(TIM3, TIM_SR_UIF);
+ // FIXME clear overflow interrupt but what else ?
+ }
+}
+
+#endif
diff --git a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.h b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.h
new file mode 100644
index 0000000000..8053ec7585
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file arch/stm32/mcu_periph/pwm_input_arch.h
+ * @ingroup stm32_arch
+ *
+ * handling of smt32 PWM input using a timer with capture.
+ */
+
+#ifndef PWM_INPUT_ARCH_H
+#define PWM_INPUT_ARCH_H
+
+#include "std.h"
+
+enum pwm_input_channels {
+ PWM_INPUT1,
+ PWM_INPUT2,
+ PWM_INPUT_NB
+};
+
+/**
+ * The pwm counter is set-up to have 1/6 us resolution.
+ *
+ * The timer clock frequency (before prescaling):
+ * STM32F1:
+ * TIM1 -> APB2 = HCLK = 72MHz
+ * TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz
+ * STM32F4:
+ * TIM1 -> 2 * APB2 = 2 * 84MHz = 168MHz
+ * TIM2 -> 2 * APB1 = 2 * 42MHz = 84MHz
+ */
+#define PWM_INPUT_TICKS_PER_USEC 6
+
+#define PWM_INPUT_TICKS_OF_USEC(_v) ((_v)*PWM_INPUT_TICKS_PER_USEC)
+#define PWM_INPUT_SIGNED_TICKS_OF_USEC(_v) (int32_t)((_v)*PWM_INPUT_TICKS_PER_USEC)
+#define USEC_OF_PWM_INPUT_TICKS(_v) ((_v)/PWM_INPUT_TICKS_PER_USEC)
+
+#include "mcu_periph/pwm_input.h"
+
+#endif /* PWM_INPUT_ARCH_H */
+
diff --git a/sw/airborne/arch/stm32/navstik.ld b/sw/airborne/arch/stm32/navstik.ld
new file mode 100644
index 0000000000..076eeac966
--- /dev/null
+++ b/sw/airborne/arch/stm32/navstik.ld
@@ -0,0 +1,35 @@
+/*
+ * Hey Emacs, this is a -*- makefile -*-
+ *
+ * Copyright (C) 2014 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.
+ */
+
+/* Linker script for Navstik (STM32F415, 1024K flash, 192K RAM). */
+
+/* Define memory regions. */
+MEMORY
+{
+ ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+ /* TODO: Pages on stm32f4 are not just 2k in size, stored settings have to deal with that correctly. */
+ rom (rx) : ORIGIN = 0x08000000, LENGTH = 1022K
+}
+
+/* Include the common ld script. */
+INCLUDE libopencm3_stm32f4.ld
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c
index 108bb9e834..a23c311aae 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c
@@ -34,6 +34,7 @@
#include
#include
+#include "mcu_periph/gpio_arch.h"
uint32_t ratio_4ms, ratio_16ms;
@@ -75,16 +76,11 @@ void actuators_dualpwm_arch_init(void) {
/*----------------
* Configure GPIO
*----------------*/
-#if defined(STM32F1)
- /* TIM3 GPIO for PWM1..4 */
- AFIO_MAPR |= AFIO_MAPR_TIM3_REMAP_FULL_REMAP;
-#endif
-
#ifdef DUAL_PWM_SERVO_5
- set_servo_gpio(DUAL_PWM_SERVO_5_GPIO, DUAL_PWM_SERVO_5_PIN, DUAL_PWM_SERVO_5_AF, DUAL_PWM_SERVO_5_RCC);
+ gpio_setup_pin_af(DUAL_PWM_SERVO_5_GPIO, DUAL_PWM_SERVO_5_PIN, DUAL_PWM_SERVO_5_AF, TRUE);
#endif
#ifdef DUAL_PWM_SERVO_6
- set_servo_gpio(DUAL_PWM_SERVO_6_GPIO, DUAL_PWM_SERVO_6_PIN, DUAL_PWM_SERVO_6_AF, DUAL_PWM_SERVO_6_RCC);
+ gpio_setup_pin_af(DUAL_PWM_SERVO_6_GPIO, DUAL_PWM_SERVO_6_PIN, DUAL_PWM_SERVO_6_AF, TRUE);
#endif
#if DUAL_PWM_USE_TIM5
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 a47b6fed50..3dcc82cb90 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -33,6 +33,8 @@
#include
#include
+#include "mcu_periph/gpio_arch.h"
+
int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
@@ -72,49 +74,43 @@ void actuators_pwm_arch_init(void) {
/*----------------
* Configure GPIO
*----------------*/
-#if defined(STM32F1)
- /* TIM3 GPIO for PWM1..4 */
- AFIO_MAPR |= AFIO_MAPR_TIM3_REMAP_FULL_REMAP;
-#endif
-
#ifdef PWM_SERVO_0
- set_servo_gpio(PWM_SERVO_0_GPIO, PWM_SERVO_0_PIN, PWM_SERVO_0_AF, PWM_SERVO_0_RCC);
+ gpio_setup_pin_af(PWM_SERVO_0_GPIO, PWM_SERVO_0_PIN, PWM_SERVO_0_AF, TRUE);
#endif
#ifdef PWM_SERVO_1
- set_servo_gpio(PWM_SERVO_1_GPIO, PWM_SERVO_1_PIN, PWM_SERVO_1_AF, PWM_SERVO_1_RCC);
+ gpio_setup_pin_af(PWM_SERVO_1_GPIO, PWM_SERVO_1_PIN, PWM_SERVO_1_AF, TRUE);
#endif
#ifdef PWM_SERVO_2
- set_servo_gpio(PWM_SERVO_2_GPIO, PWM_SERVO_2_PIN, PWM_SERVO_2_AF, PWM_SERVO_2_RCC);
+ gpio_setup_pin_af(PWM_SERVO_2_GPIO, PWM_SERVO_2_PIN, PWM_SERVO_2_AF, TRUE);
#endif
#ifdef PWM_SERVO_3
- set_servo_gpio(PWM_SERVO_3_GPIO, PWM_SERVO_3_PIN, PWM_SERVO_3_AF, PWM_SERVO_3_RCC);
+ gpio_setup_pin_af(PWM_SERVO_3_GPIO, PWM_SERVO_3_PIN, PWM_SERVO_3_AF, TRUE);
#endif
#ifdef PWM_SERVO_4
- set_servo_gpio(PWM_SERVO_4_GPIO, PWM_SERVO_4_PIN, PWM_SERVO_4_AF, PWM_SERVO_4_RCC);
+ gpio_setup_pin_af(PWM_SERVO_4_GPIO, PWM_SERVO_4_PIN, PWM_SERVO_4_AF, TRUE);
#endif
#ifdef PWM_SERVO_5
- set_servo_gpio(PWM_SERVO_5_GPIO, PWM_SERVO_5_PIN, PWM_SERVO_5_AF, PWM_SERVO_5_RCC);
+ gpio_setup_pin_af(PWM_SERVO_5_GPIO, PWM_SERVO_5_PIN, PWM_SERVO_5_AF, TRUE);
#endif
#ifdef PWM_SERVO_6
- set_servo_gpio(PWM_SERVO_6_GPIO, PWM_SERVO_6_PIN, PWM_SERVO_6_AF, PWM_SERVO_6_RCC);
+ gpio_setup_pin_af(PWM_SERVO_6_GPIO, PWM_SERVO_6_PIN, PWM_SERVO_6_AF, TRUE);
#endif
#ifdef PWM_SERVO_7
- set_servo_gpio(PWM_SERVO_7_GPIO, PWM_SERVO_7_PIN, PWM_SERVO_7_AF, PWM_SERVO_7_RCC);
+ gpio_setup_pin_af(PWM_SERVO_7_GPIO, PWM_SERVO_7_PIN, PWM_SERVO_7_AF, TRUE);
#endif
#ifdef PWM_SERVO_8
- set_servo_gpio(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, PWM_SERVO_8_RCC);
+ gpio_setup_pin_af(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, TRUE);
#endif
#ifdef PWM_SERVO_9
- set_servo_gpio(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, PWM_SERVO_9_RCC);
+ gpio_setup_pin_af(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, TRUE);
#endif
#ifdef PWM_SERVO_10
- set_servo_gpio(PWM_SERVO_10_GPIO, PWM_SERVO_10_PIN, PWM_SERVO_10_AF, PWM_SERVO_10_RCC);
+ gpio_setup_pin_af(PWM_SERVO_10_GPIO, PWM_SERVO_10_PIN, PWM_SERVO_10_AF, TRUE);
#endif
#ifdef PWM_SERVO_11
- set_servo_gpio(PWM_SERVO_11_GPIO, PWM_SERVO_11_PIN, PWM_SERVO_11_AF, PWM_SERVO_11_RCC);
+ gpio_setup_pin_af(PWM_SERVO_11_GPIO, PWM_SERVO_11_PIN, PWM_SERVO_11_AF, TRUE);
#endif
-
#if PWM_USE_TIM1
set_servo_timer(TIM1, TIM1_SERVO_HZ, PWM_TIM1_CHAN_MASK);
#endif
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c
index 08be5cd2de..42e15146e0 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c
@@ -28,24 +28,6 @@
// for timer_get_frequency
#include "mcu_arch.h"
-/** Set GPIO configuration
- */
-#if defined(STM32F4)
-void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, enum rcc_periph_clken clken) {
- rcc_periph_clock_enable(clken);
- gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin);
- gpio_set_af(gpioport, af_num, pin);
-}
-#elif defined(STM32F1)
-void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none __attribute__((unused)), enum rcc_periph_clken clken) {
- rcc_periph_clock_enable(clken);
- rcc_periph_clock_enable(RCC_AFIO);
- gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ,
- GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin);
-}
-#endif
-
-
/** Set PWM channel configuration
*/
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h
index 1e9d302e6b..acc9590639 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h
@@ -36,15 +36,6 @@
#include
#include "mcu_arch.h"
-
-#if defined(STM32F1)
-//#define PCLK 72000000
-#define PCLK AHB_CLK
-#elif defined(STM32F4)
-//#define PCLK 84000000
-#define PCLK AHB_CLK/2
-#endif
-
#define ONE_MHZ_CLK 1000000
/* Default timer base frequency is 1MHz */
@@ -74,6 +65,9 @@
#ifndef TIM5_SERVO_HZ
#define TIM5_SERVO_HZ SERVO_HZ
#endif
+#ifndef TIM8_SERVO_HZ
+#define TIM8_SERVO_HZ SERVO_HZ
+#endif
#ifndef TIM9_SERVO_HZ
#define TIM9_SERVO_HZ SERVO_HZ
#endif
@@ -81,15 +75,7 @@
#define TIM12_SERVO_HZ SERVO_HZ
#endif
-
-#if defined(STM32F4)
-extern void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, enum rcc_periph_clken clken);
-#elif defined(STM32F1)
-extern void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none __attribute__((unused)), enum rcc_periph_clken clken);
-#endif
-
extern void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, enum tim_oc_id oc_id);
-
extern void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask);
#endif /* ACTUATORS_PWM_SHARED_ARCH_H */
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
index 4536f3bd82..7bb3eb1377 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
@@ -691,8 +691,13 @@ void radio_control_spektrum_try_bind(void) {
/* exit if the BIND_PIN is high, it needs to
be pulled low at startup to initiate bind */
+#ifdef SPEKTRUM_BIND_PIN_HIGH
+ if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0)
+ return;
+#else
if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) != 0)
return;
+#endif
/* Master receiver Rx push-pull */
gpio_setup_output(SPEKTRUM_PRIMARY_BIND_CONF_PORT, SPEKTRUM_PRIMARY_BIND_CONF_PIN);
diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h
index 7b32b0213e..70650327fb 100644
--- a/sw/airborne/boards/apogee_0.99.h
+++ b/sw/airborne/boards/apogee_0.99.h
@@ -165,7 +165,6 @@
#if USE_PWM0
#define PWM_SERVO_0 0
#define PWM_SERVO_0_TIMER TIM2
-#define PWM_SERVO_0_RCC RCC_GPIOA
#define PWM_SERVO_0_GPIO GPIOA
#define PWM_SERVO_0_PIN GPIO3
#define PWM_SERVO_0_AF GPIO_AF1
@@ -178,7 +177,6 @@
#if USE_PWM1
#define PWM_SERVO_1 1
#define PWM_SERVO_1_TIMER TIM2
-#define PWM_SERVO_1_RCC RCC_GPIOA
#define PWM_SERVO_1_GPIO GPIOA
#define PWM_SERVO_1_PIN GPIO2
#define PWM_SERVO_1_AF GPIO_AF1
@@ -191,7 +189,6 @@
#if USE_PWM2
#define PWM_SERVO_2 2
#define PWM_SERVO_2_TIMER TIM3
-#define PWM_SERVO_2_RCC RCC_GPIOB
#define PWM_SERVO_2_GPIO GPIOB
#define PWM_SERVO_2_PIN GPIO5
#define PWM_SERVO_2_AF GPIO_AF2
@@ -204,7 +201,6 @@
#if USE_PWM3
#define PWM_SERVO_3_IDX 3
#define PWM_SERVO_3_TIMER TIM3
-#define PWM_SERVO_3_RCC RCC_GPIOB
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO4
#define PWM_SERVO_3_AF GPIO_AF2
@@ -217,7 +213,6 @@
#if USE_PWM4
#define PWM_SERVO_4 4
#define PWM_SERVO_4_TIMER TIM2
-#define PWM_SERVO_4_RCC RCC_GPIOB
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO3
#define PWM_SERVO_4_AF GPIO_AF1
@@ -230,7 +225,6 @@
#if USE_PWM5
#define PWM_SERVO_5 5
#define PWM_SERVO_5_TIMER TIM2
-#define PWM_SERVO_5_RCC RCC_GPIOA
#define PWM_SERVO_5_GPIO GPIOA
#define PWM_SERVO_5_PIN GPIO15
#define PWM_SERVO_5_AF GPIO_AF1
diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h
index 92cf499abc..4577f76266 100644
--- a/sw/airborne/boards/apogee_1.0.h
+++ b/sw/airborne/boards/apogee_1.0.h
@@ -222,7 +222,6 @@
#if USE_PWM0
#define PWM_SERVO_0 0
#define PWM_SERVO_0_TIMER TIM3
-#define PWM_SERVO_0_RCC RCC_GPIOB
#define PWM_SERVO_0_GPIO GPIOB
#define PWM_SERVO_0_PIN GPIO0
#define PWM_SERVO_0_AF GPIO_AF2
@@ -238,7 +237,6 @@
#if USE_PWM1
#define PWM_SERVO_1 1
#define PWM_SERVO_1_TIMER TIM2
-#define PWM_SERVO_1_RCC RCC_GPIOA
#define PWM_SERVO_1_GPIO GPIOA
#define PWM_SERVO_1_PIN GPIO2
#define PWM_SERVO_1_AF GPIO_AF1
@@ -254,7 +252,6 @@
#if USE_PWM2
#define PWM_SERVO_2 2
#define PWM_SERVO_2_TIMER TIM3
-#define PWM_SERVO_2_RCC RCC_GPIOB
#define PWM_SERVO_2_GPIO GPIOB
#define PWM_SERVO_2_PIN GPIO5
#define PWM_SERVO_2_AF GPIO_AF2
@@ -270,7 +267,6 @@
#if USE_PWM3
#define PWM_SERVO_3 3
#define PWM_SERVO_3_TIMER TIM3
-#define PWM_SERVO_3_RCC RCC_GPIOB
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO4
#define PWM_SERVO_3_AF GPIO_AF2
@@ -286,7 +282,6 @@
#if USE_PWM4
#define PWM_SERVO_4 4
#define PWM_SERVO_4_TIMER TIM2
-#define PWM_SERVO_4_RCC RCC_GPIOB
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO3
#define PWM_SERVO_4_AF GPIO_AF1
@@ -302,7 +297,6 @@
#if USE_PWM5
#define PWM_SERVO_5 5
#define PWM_SERVO_5_TIMER TIM2
-#define PWM_SERVO_5_RCC RCC_GPIOA
#define PWM_SERVO_5_GPIO GPIOA
#define PWM_SERVO_5_PIN GPIO15
#define PWM_SERVO_5_AF GPIO_AF1
@@ -316,7 +310,6 @@
#if USE_PWM6
#define PWM_SERVO_6 6
#define PWM_SERVO_6_TIMER TIM3
-#define PWM_SERVO_6_RCC RCC_GPIOB
#define PWM_SERVO_6_GPIO GPIOB
#define PWM_SERVO_6_PIN GPIO1
#define PWM_SERVO_6_AF GPIO_AF2
@@ -346,6 +339,28 @@
#define PPM_GPIO_PIN GPIO8
#define PPM_GPIO_AF GPIO_AF1
+/*
+ * PWM input
+ */
+#define PWM_INPUT1_TIMER TIM1
+#define PWM_INPUT1_CHANNEL_PERIOD TIM_IC1
+#define PWM_INPUT1_CHANNEL_DUTY TIM_IC2
+#define PWM_INPUT1_TIMER_INPUT TIM_IC_IN_TI1
+#define PWM_INPUT1_SLAVE_TRIG TIM_SMCR_TS_IT1FP1
+#define PWM_INPUT1_IRQ NVIC_TIM1_CC_IRQ
+#define PWM_INPUT1_IRQ2 NVIC_TIM1_UP_TIM10_IRQ
+#define PWM_INPUT1_CC_IE (TIM_DIER_CC1IE | TIM_DIER_CC2IE)
+#define USE_PWM_INPUT_TIM1 TRUE
+#define TIM1_PWM_INPUT_IDX 0
+#define TIM1_CC_IF_PERIOD TIM_SR_CC1IF
+#define TIM1_CC_IF_DUTY TIM_SR_CC2IF
+#define TIM1_CCR_PERIOD TIM1_CCR1
+#define TIM1_CCR_DUTY TIM1_CCR2
+// PPM in (aka PA8) is used: not compatible with PPM RC receiver
+#define PWM_INPUT1_GPIO_PORT GPIOA
+#define PWM_INPUT1_GPIO_PIN GPIO8
+#define PWM_INPUT1_GPIO_AF GPIO_AF1
+
/*
* Spektrum
*/
diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h
index 4a64be651f..19127a937a 100644
--- a/sw/airborne/boards/krooz_sd.h
+++ b/sw/airborne/boards/krooz_sd.h
@@ -219,7 +219,6 @@
#if USE_PWM0
#define PWM_SERVO_0 0
#define PWM_SERVO_0_TIMER TIM3
-#define PWM_SERVO_0_RCC RCC_GPIOB
#define PWM_SERVO_0_GPIO GPIOB
#define PWM_SERVO_0_PIN GPIO1
#define PWM_SERVO_0_AF GPIO_AF2
@@ -232,7 +231,6 @@
#if USE_PWM1
#define PWM_SERVO_1 1
#define PWM_SERVO_1_TIMER TIM3
-#define PWM_SERVO_1_RCC RCC_GPIOC
#define PWM_SERVO_1_GPIO GPIOC
#define PWM_SERVO_1_PIN GPIO8
#define PWM_SERVO_1_AF GPIO_AF2
@@ -245,7 +243,6 @@
#if USE_PWM2
#define PWM_SERVO_2 2
#define PWM_SERVO_2_TIMER TIM3
-#define PWM_SERVO_2_RCC RCC_GPIOC
#define PWM_SERVO_2_GPIO GPIOC
#define PWM_SERVO_2_PIN GPIO7
#define PWM_SERVO_2_AF GPIO_AF2
@@ -258,7 +255,6 @@
#if USE_PWM3
#define PWM_SERVO_3 3
#define PWM_SERVO_3_TIMER TIM3
-#define PWM_SERVO_3_RCC RCC_GPIOB
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO4
#define PWM_SERVO_3_AF GPIO_AF2
@@ -271,7 +267,6 @@
#if USE_PWM4
#define PWM_SERVO_4 4
#define PWM_SERVO_4_TIMER TIM4
-#define PWM_SERVO_4_RCC RCC_GPIOB
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO7
#define PWM_SERVO_4_AF GPIO_AF2
@@ -284,7 +279,6 @@
#if USE_PWM5
#define PWM_SERVO_5 5
#define PWM_SERVO_5_TIMER TIM4
-#define PWM_SERVO_5_RCC RCC_GPIOB
#define PWM_SERVO_5_GPIO GPIOB
#define PWM_SERVO_5_PIN GPIO6
#define PWM_SERVO_5_AF GPIO_AF2
@@ -297,7 +291,6 @@
#if USE_PWM6
#define PWM_SERVO_6 6
#define PWM_SERVO_6_TIMER TIM5
-#define PWM_SERVO_6_RCC RCC_GPIOA
#define PWM_SERVO_6_GPIO GPIOA
#define PWM_SERVO_6_PIN GPIO3
#define PWM_SERVO_6_AF GPIO_AF2
@@ -310,7 +303,6 @@
#if USE_PWM7
#define PWM_SERVO_7 7
#define PWM_SERVO_7_TIMER TIM5
-#define PWM_SERVO_7_RCC RCC_GPIOA
#define PWM_SERVO_7_GPIO GPIOA
#define PWM_SERVO_7_PIN GPIO2
#define PWM_SERVO_7_AF GPIO_AF2
@@ -323,7 +315,6 @@
#if USE_PWM8
#define PWM_SERVO_8 8
#define PWM_SERVO_8_TIMER TIM5
-#define PWM_SERVO_8_RCC RCC_GPIOA
#define PWM_SERVO_8_GPIO GPIOA
#define PWM_SERVO_8_PIN GPIO1
#define PWM_SERVO_8_AF GPIO_AF2
@@ -336,7 +327,6 @@
#if USE_PWM9
#define PWM_SERVO_9 9
#define PWM_SERVO_9_TIMER TIM5
-#define PWM_SERVO_9_RCC RCC_GPIOA
#define PWM_SERVO_9_GPIO GPIOA
#define PWM_SERVO_9_PIN GPIO0
#define PWM_SERVO_9_AF GPIO_AF2
@@ -349,7 +339,6 @@
#if USE_PWM10
#define PWM_SERVO_10 10
#define PWM_SERVO_10_TIMER TIM2
-#define PWM_SERVO_10_RCC RCC_GPIOB
#define PWM_SERVO_10_GPIO GPIOB
#define PWM_SERVO_10_PIN GPIO3
#define PWM_SERVO_10_AF GPIO_AF1
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index b8aa6cba2f..a9de2af1e0 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -203,10 +203,9 @@
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM3
-#define PWM_SERVO_1_RCC RCC_GPIOC
#define PWM_SERVO_1_GPIO GPIOC
#define PWM_SERVO_1_PIN GPIO6
-#define PWM_SERVO_1_AF 0
+#define PWM_SERVO_1_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_1_OC TIM_OC1
#define PWM_SERVO_1_OC_BIT (1<<0)
#else
@@ -216,10 +215,9 @@
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM3
-#define PWM_SERVO_2_RCC RCC_GPIOC
#define PWM_SERVO_2_GPIO GPIOC
#define PWM_SERVO_2_PIN GPIO7
-#define PWM_SERVO_2_AF 0
+#define PWM_SERVO_2_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_2_OC TIM_OC2
#define PWM_SERVO_2_OC_BIT (1<<1)
#else
@@ -229,10 +227,9 @@
#if USE_PWM3
#define PWM_SERVO_3 2
#define PWM_SERVO_3_TIMER TIM3
-#define PWM_SERVO_3_RCC RCC_GPIOC
#define PWM_SERVO_3_GPIO GPIOC
#define PWM_SERVO_3_PIN GPIO8
-#define PWM_SERVO_3_AF 0
+#define PWM_SERVO_3_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_3_OC TIM_OC3
#define PWM_SERVO_3_OC_BIT (1<<2)
#else
@@ -242,10 +239,9 @@
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM3
-#define PWM_SERVO_4_RCC RCC_GPIOC
#define PWM_SERVO_4_GPIO GPIOC
#define PWM_SERVO_4_PIN GPIO9
-#define PWM_SERVO_4_AF 0
+#define PWM_SERVO_4_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_4_OC TIM_OC4
#define PWM_SERVO_4_OC_BIT (1<<3)
#else
@@ -255,7 +251,6 @@
#if USE_PWM5
#define PWM_SERVO_5 4
#define PWM_SERVO_5_TIMER TIM4
-#define PWM_SERVO_5_RCC RCC_GPIOB
#define PWM_SERVO_5_GPIO GPIOB
#define PWM_SERVO_5_PIN GPIO8
#define PWM_SERVO_5_AF 0
@@ -268,12 +263,11 @@
#if USE_PWM6
#define PWM_SERVO_6 5
#define PWM_SERVO_6_TIMER TIM4
-#define PWM_SERVO_6_RCC RCC_GPIOB
#define PWM_SERVO_6_GPIO GPIOB
#define PWM_SERVO_6_PIN GPIO9
#define PWM_SERVO_6_AF 0
#define PWM_SERVO_6_OC TIM_OC4
-#define PWM_SERVO_6_OC_BIT (1<<4)
+#define PWM_SERVO_6_OC_BIT (1<<3)
#else
#define PWM_SERVO_6_OC_BIT 0
#endif
diff --git a/sw/airborne/boards/lisa_m_common.h b/sw/airborne/boards/lisa_m_common.h
index 6f0a26da27..fd1610de05 100644
--- a/sw/airborne/boards/lisa_m_common.h
+++ b/sw/airborne/boards/lisa_m_common.h
@@ -256,10 +256,9 @@
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM3
-#define PWM_SERVO_1_RCC RCC_GPIOC
#define PWM_SERVO_1_GPIO GPIOC
#define PWM_SERVO_1_PIN GPIO6
-#define PWM_SERVO_1_AF 0
+#define PWM_SERVO_1_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_1_OC TIM_OC1
#define PWM_SERVO_1_OC_BIT (1<<0)
#else
@@ -269,10 +268,9 @@
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM3
-#define PWM_SERVO_2_RCC RCC_GPIOC
#define PWM_SERVO_2_GPIO GPIOC
#define PWM_SERVO_2_PIN GPIO7
-#define PWM_SERVO_2_AF 0
+#define PWM_SERVO_2_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_2_OC TIM_OC2
#define PWM_SERVO_2_OC_BIT (1<<1)
#else
@@ -282,10 +280,9 @@
#if USE_PWM3
#define PWM_SERVO_3 2
#define PWM_SERVO_3_TIMER TIM3
-#define PWM_SERVO_3_RCC RCC_GPIOC
#define PWM_SERVO_3_GPIO GPIOC
#define PWM_SERVO_3_PIN GPIO8
-#define PWM_SERVO_3_AF 0
+#define PWM_SERVO_3_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_3_OC TIM_OC3
#define PWM_SERVO_3_OC_BIT (1<<2)
#else
@@ -295,10 +292,9 @@
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM3
-#define PWM_SERVO_4_RCC RCC_GPIOC
#define PWM_SERVO_4_GPIO GPIOC
#define PWM_SERVO_4_PIN GPIO9
-#define PWM_SERVO_4_AF 0
+#define PWM_SERVO_4_AF AFIO_MAPR_TIM3_REMAP_FULL_REMAP
#define PWM_SERVO_4_OC TIM_OC4
#define PWM_SERVO_4_OC_BIT (1<<3)
#else
@@ -308,7 +304,6 @@
#if USE_PWM5
#define PWM_SERVO_5 4
#define PWM_SERVO_5_TIMER TIM5
- #define PWM_SERVO_5_RCC RCC_GPIOA
#define PWM_SERVO_5_GPIO GPIOA
#define PWM_SERVO_5_PIN GPIO0
#define PWM_SERVO_5_AF 0
@@ -333,7 +328,6 @@
#if USE_PWM6
#define PWM_SERVO_6 5
#define PWM_SERVO_6_TIMER TIM5
- #define PWM_SERVO_6_RCC RCC_GPIOA
#define PWM_SERVO_6_GPIO GPIOA
#define PWM_SERVO_6_PIN GPIO1
#define PWM_SERVO_6_AF 0
@@ -363,7 +357,6 @@
#if USE_PWM7
#define PWM_SERVO_7 6
#define PWM_SERVO_7_TIMER TIM4
-#define PWM_SERVO_7_RCC RCC_GPIOB
#define PWM_SERVO_7_GPIO GPIOB
#define PWM_SERVO_7_PIN GPIO6
#define PWM_SERVO_7_AF 0
@@ -376,7 +369,6 @@
#if USE_PWM8
#define PWM_SERVO_8 7
#define PWM_SERVO_8_TIMER TIM4
-#define PWM_SERVO_8_RCC RCC_GPIOB
#define PWM_SERVO_8_GPIO GPIOB
#define PWM_SERVO_8_PIN GPIO7
#define PWM_SERVO_8_AF 0
diff --git a/sw/airborne/boards/lisa_mx_2.0.h b/sw/airborne/boards/lisa_mx_2.0.h
index 33c85c4042..ca74a4ffec 100644
--- a/sw/airborne/boards/lisa_mx_2.0.h
+++ b/sw/airborne/boards/lisa_mx_2.0.h
@@ -382,7 +382,6 @@
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM3
-#define PWM_SERVO_1_RCC RCC_GPIOC
#define PWM_SERVO_1_GPIO GPIOC
#define PWM_SERVO_1_PIN GPIO6
#define PWM_SERVO_1_AF GPIO_AF2
@@ -395,7 +394,6 @@
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM3
-#define PWM_SERVO_2_RCC RCC_GPIOC
#define PWM_SERVO_2_GPIO GPIOC
#define PWM_SERVO_2_PIN GPIO7
#define PWM_SERVO_2_AF GPIO_AF2
@@ -408,7 +406,6 @@
#if USE_PWM3
#define PWM_SERVO_3 2
#define PWM_SERVO_3_TIMER TIM3
-#define PWM_SERVO_3_RCC RCC_GPIOC
#define PWM_SERVO_3_GPIO GPIOC
#define PWM_SERVO_3_PIN GPIO8
#define PWM_SERVO_3_AF GPIO_AF2
@@ -421,7 +418,6 @@
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM3
-#define PWM_SERVO_4_RCC RCC_GPIOC
#define PWM_SERVO_4_GPIO GPIOC
#define PWM_SERVO_4_PIN GPIO9
#define PWM_SERVO_4_AF GPIO_AF2
@@ -434,7 +430,6 @@
#if USE_PWM5
#define PWM_SERVO_5 4
#define PWM_SERVO_5_TIMER TIM5
-#define PWM_SERVO_5_RCC RCC_GPIOA
#define PWM_SERVO_5_GPIO GPIOA
#define PWM_SERVO_5_PIN GPIO0
#define PWM_SERVO_5_AF GPIO_AF2
@@ -447,7 +442,6 @@
#if USE_PWM6
#define PWM_SERVO_6 5
#define PWM_SERVO_6_TIMER TIM5
-#define PWM_SERVO_6_RCC RCC_GPIOA
#define PWM_SERVO_6_GPIO GPIOA
#define PWM_SERVO_6_PIN GPIO1
#define PWM_SERVO_6_AF GPIO_AF2
@@ -460,7 +454,6 @@
#if USE_PWM7
#define PWM_SERVO_7 6
#define PWM_SERVO_7_TIMER TIM4
-#define PWM_SERVO_7_RCC RCC_GPIOB
#define PWM_SERVO_7_GPIO GPIOB
#define PWM_SERVO_7_PIN GPIO6
#define PWM_SERVO_7_AF GPIO_AF2
@@ -473,7 +466,6 @@
#if USE_PWM8
#define PWM_SERVO_8 7
#define PWM_SERVO_8_TIMER TIM4
-#define PWM_SERVO_8_RCC RCC_GPIOB
#define PWM_SERVO_8_GPIO GPIOB
#define PWM_SERVO_8_PIN GPIO7
#define PWM_SERVO_8_AF GPIO_AF2
diff --git a/sw/airborne/boards/lisa_s_1.0.h b/sw/airborne/boards/lisa_s_1.0.h
index bf1fa624b7..9316fbf413 100644
--- a/sw/airborne/boards/lisa_s_1.0.h
+++ b/sw/airborne/boards/lisa_s_1.0.h
@@ -315,7 +315,6 @@
#if USE_PWM1
#define PWM_SERVO_1 4
#define PWM_SERVO_1_TIMER TIM4
-#define PWM_SERVO_1_RCC RCC_GPIOB
#define PWM_SERVO_1_GPIO GPIOB
#define PWM_SERVO_1_PIN GPIO6
#define PWM_SERVO_1_AF 0
@@ -328,7 +327,6 @@
#if USE_PWM2
#define PWM_SERVO_2 5
#define PWM_SERVO_2_TIMER TIM4
-#define PWM_SERVO_2_RCC RCC_GPIOB
#define PWM_SERVO_2_GPIO GPIOB
#define PWM_SERVO_2_PIN GPIO7
#define PWM_SERVO_2_AF 0
@@ -341,7 +339,6 @@
#if USE_PWM3
#define PWM_SERVO_3 0
#define PWM_SERVO_3_TIMER TIM4
-#define PWM_SERVO_3_RCC RCC_GPIOB
#define PWM_SERVO_3_GPIO GPIOB
#define PWM_SERVO_3_PIN GPIO8
#define PWM_SERVO_3_AF 0
@@ -354,7 +351,6 @@
#if USE_PWM4
#define PWM_SERVO_4 1
#define PWM_SERVO_4_TIMER TIM4
-#define PWM_SERVO_4_RCC RCC_GPIOB
#define PWM_SERVO_4_GPIO GPIOB
#define PWM_SERVO_4_PIN GPIO9
#define PWM_SERVO_4_AF 0
@@ -367,7 +363,6 @@
#if USE_PWM5
#define PWM_SERVO_5 2
#define PWM_SERVO_5_TIMER TIM5
- #define PWM_SERVO_5_RCC RCC_GPIOA
#define PWM_SERVO_5_GPIO GPIOA
#define PWM_SERVO_5_PIN GPIO0
#define PWM_SERVO_5_AF 0
@@ -392,7 +387,6 @@
#if USE_PWM6
#define PWM_SERVO_6 3
#define PWM_SERVO_6_TIMER TIM5
- #define PWM_SERVO_6_RCC RCC_GPIOA
#define PWM_SERVO_6_GPIO GPIOA
#define PWM_SERVO_6_PIN GPIO1
#define PWM_SERVO_6_AF 0
diff --git a/sw/airborne/boards/navstik/baro_board.c b/sw/airborne/boards/navstik/baro_board.c
new file mode 100644
index 0000000000..b0d030e2ec
--- /dev/null
+++ b/sw/airborne/boards/navstik/baro_board.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2014 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 boards/navstik/baro_board.c
+ * Baro board interface for Bosch BMP185 on Navstik I2C3 without EOC check.
+ */
+
+#include "std.h"
+
+#include "subsystems/sensors/baro.h"
+#include "peripherals/bmp085.h"
+#include "peripherals/bmp085_regs.h"
+#include
+#include "subsystems/abi.h"
+
+#include "led.h"
+
+
+struct Bmp085 baro_bmp085;
+
+void baro_init(void) {
+ bmp085_init(&baro_bmp085, &i2c3, BMP085_SLAVE_ADDR);
+
+#ifdef BARO_LED
+ LED_OFF(BARO_LED);
+#endif
+}
+
+void baro_periodic(void) {
+ if (baro_bmp085.initialized) {
+ bmp085_periodic(&baro_bmp085);
+ }
+ else {
+ bmp085_read_eeprom_calib(&baro_bmp085);
+ }
+}
+
+void baro_event(void) {
+ bmp085_event(&baro_bmp085);
+
+ if (baro_bmp085.data_available) {
+ float pressure = (float)baro_bmp085.pressure;
+ AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
+ baro_bmp085.data_available = FALSE;
+#ifdef BARO_LED
+ RunOnceEvery(10,LED_TOGGLE(BARO_LED));
+#endif
+ }
+}
diff --git a/sw/airborne/boards/navstik/baro_board.h b/sw/airborne/boards/navstik/baro_board.h
new file mode 100644
index 0000000000..b213ea597d
--- /dev/null
+++ b/sw/airborne/boards/navstik/baro_board.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2014 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.
+ */
+
+/*
+ * board specific functions for the navstik board
+ */
+
+#ifndef BOARDS_NAVSTIK_BARO_H
+#define BOARDS_NAVSTIK_BARO_H
+
+// only for printing the baro type during compilation
+#ifndef BARO_BOARD
+#define BARO_BOARD BARO_BOARD_BMP085
+#endif
+
+extern void baro_event(void);
+#define BaroEvent baro_event
+
+#endif /* BOARDS_NAVSTIK_BARO_H */
diff --git a/sw/airborne/boards/navstik_1.0.h b/sw/airborne/boards/navstik_1.0.h
new file mode 100644
index 0000000000..db0fd1d04f
--- /dev/null
+++ b/sw/airborne/boards/navstik_1.0.h
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2014 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.
+ *
+ */
+
+#ifndef CONFIG_NAVSTIK_1_0_H
+#define CONFIG_NAVSTIK_1_0_H
+
+/* Navstik has a 12MHz external clock and 168MHz internal. */
+#define EXT_CLK 25000000
+#define AHB_CLK 168000000
+
+/*
+ * Onboard LEDs
+ */
+
+/* red, on PA8 */
+#ifndef USE_LED_1
+#define USE_LED_1 1
+#endif
+#define LED_1_GPIO GPIOC
+#define LED_1_GPIO_PIN GPIO4
+#define LED_1_GPIO_ON gpio_set
+#define LED_1_GPIO_OFF gpio_clear
+#define LED_1_AFIO_REMAP ((void)0)
+
+/* green, shared with JTAG_TRST */
+#ifndef USE_LED_2
+#define USE_LED_2 1
+#endif
+#define LED_2_GPIO GPIOC
+#define LED_2_GPIO_PIN GPIO5
+#define LED_2_GPIO_ON gpio_set
+#define LED_2_GPIO_OFF gpio_clear
+#define LED_2_AFIO_REMAP ((void)0)
+
+
+/*
+ * not actual LEDS, used as GPIOs
+ */
+#define GPS_POWER_GPIO GPIOA,GPIO4
+#define IMU_POWER_GPIO GPIOC,GPIO15
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+
+/* UART */
+#define UART1_GPIO_AF GPIO_AF7
+#define UART1_GPIO_PORT_RX GPIOB
+#define UART1_GPIO_RX GPIO7
+#define UART1_GPIO_PORT_TX GPIOB
+#define UART1_GPIO_TX GPIO6
+
+#define UART2_GPIO_AF GPIO_AF7
+#define UART2_GPIO_PORT_RX GPIOA
+#define UART2_GPIO_RX GPIO3
+#define UART2_GPIO_PORT_TX GPIOA
+#define UART2_GPIO_TX GPIO2
+
+#define UART5_GPIO_AF GPIO_AF8
+#define UART5_GPIO_PORT_RX GPIOD
+#define UART5_GPIO_RX GPIO2
+#define UART5_GPIO_PORT_TX GPIOC
+#define UART5_GPIO_TX GPIO12
+
+#define UART6_GPIO_AF GPIO_AF8
+#define UART6_GPIO_PORT_RX GPIOC
+#define UART6_GPIO_RX GPIO7
+#define UART6_GPIO_PORT_TX GPIOC
+#define UART6_GPIO_TX GPIO6
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO6
+#define SPEKTRUM_BIND_PIN_PORT GPIOC
+#define SPEKTRUM_BIND_PIN_HIGH 1
+
+#define SPEKTRUM_UART6_RCC RCC_USART6
+#define SPEKTRUM_UART6_BANK GPIOC
+#define SPEKTRUM_UART6_PIN GPIO7
+#define SPEKTRUM_UART6_AF GPIO_AF8
+#define SPEKTRUM_UART6_IRQ NVIC_USART6_IRQ
+#define SPEKTRUM_UART6_ISR usart6_isr
+#define SPEKTRUM_UART6_DEV USART6
+
+/*
+ * PPM
+ */
+
+/* input on PC6 (Spektrum Tx) */
+#define USE_PPM_TIM8 8
+#define PPM_CHANNEL TIM_IC3
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
+#define PPM_IRQ NVIC_TIM3_CC_IRQ
+#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC2IE
+#define PPM_CC_IF TIM_SR_CC2IF
+#define PPM_GPIO_PORT GPIOC
+#define PPM_GPIO_PIN GPIO7
+#define PPM_GPIO_AF GPIO_AF8
+
+/* SPI */
+#define SPI1_GPIO_AF GPIO_AF5
+#define SPI1_GPIO_PORT_MISO GPIOA
+#define SPI1_GPIO_MISO GPIO6
+#define SPI1_GPIO_PORT_MOSI GPIOA
+#define SPI1_GPIO_MOSI GPIO7
+#define SPI1_GPIO_PORT_SCK GPIOA
+#define SPI1_GPIO_SCK GPIO5
+
+#define SPI2_GPIO_AF GPIO_AF5
+#define SPI2_GPIO_PORT_MISO GPIOB
+#define SPI2_GPIO_MISO GPIO14
+#define SPI2_GPIO_PORT_MOSI GPIOB
+#define SPI2_GPIO_MOSI GPIO15
+#define SPI2_GPIO_PORT_SCK GPIOB
+#define SPI2_GPIO_SCK GPIO13
+
+#define SPI_SELECT_SLAVE0_PORT GPIOA
+#define SPI_SELECT_SLAVE0_PIN GPIO15
+
+#define SPI_SELECT_SLAVE1_PORT GPIOA
+#define SPI_SELECT_SLAVE1_PIN GPIO4
+
+#define SPI_SELECT_SLAVE2_PORT GPIOB
+#define SPI_SELECT_SLAVE2_PIN GPIO12
+
+#define SPI_SELECT_SLAVE3_PORT GPIOC
+#define SPI_SELECT_SLAVE3_PIN GPIO13
+
+#define SPI_SELECT_SLAVE4_PORT GPIOC
+#define SPI_SELECT_SLAVE4_PIN GPIO12
+
+#define SPI_SELECT_SLAVE5_PORT GPIOC
+#define SPI_SELECT_SLAVE5_PIN GPIO4
+
+
+/* I2C mapping */
+#define I2C1_GPIO_PORT GPIOB
+#define I2C1_GPIO_SCL GPIO8
+#define I2C1_GPIO_SDA GPIO9
+
+#define I2C3_GPIO_PORT_SCL GPIOA
+#define I2C3_GPIO_SCL GPIO8
+#define I2C3_GPIO_PORT_SDA GPIOC
+#define I2C3_GPIO_SDA GPIO9
+
+
+/*
+ * ADC
+ */
+
+/* Onboard ADCs */
+/*
+ BATT_volt PC1/ADC123 (ADC123_IN11)
+ BATT_current PA1/ADC123 (ADC123_IN1)
+*/
+
+// Internal ADC for battery enabled by default
+#ifndef USE_ADC_1
+#define USE_ADC_1 1
+#endif
+#if USE_ADC_1
+#define AD1_1_CHANNEL 11
+#define ADC_1 AD1_1
+#define ADC_1_GPIO_PORT GPIOC
+#define ADC_1_GPIO_PIN GPIO1
+#endif
+
+#ifndef USE_ADC_2
+#define USE_ADC_2 1
+#endif
+#if USE_ADC_2
+#define AD1_2_CHANNEL 1
+#define ADC_2 AD1_2
+#define ADC_2_GPIO_PORT GPIOA
+#define ADC_2_GPIO_PIN GPIO1
+#endif
+
+/* allow to define ADC_CHANNEL_VSUPPLY and ADC_CHANNEL_CURRENT in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY ADC_1
+#endif
+#ifndef ADC_CHANNEL_CURRENT
+#define ADC_CHANNEL_CURRENT ADC_2
+#endif
+
+#define DefaultVoltageOfAdc(adc) (0.00382*adc)
+#define DefaultMilliAmpereOfAdc(adc) (0.42497*adc)
+
+
+/*
+ * PWM
+ *
+ */
+#define PWM_USE_TIM1 1
+#define PWM_USE_TIM2 2
+#define PWM_USE_TIM3 3
+#define PWM_USE_TIM8 8
+
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+#define USE_PWM6 1
+
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+#if USE_PWM1
+#define PWM_SERVO_1 0
+#define PWM_SERVO_1_TIMER TIM3
+#define PWM_SERVO_1_GPIO GPIOB
+#define PWM_SERVO_1_PIN GPIO5
+#define PWM_SERVO_1_AF GPIO_AF2
+#define PWM_SERVO_1_OC TIM_OC2
+#define PWM_SERVO_1_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 1
+#define PWM_SERVO_2_TIMER TIM1
+#define PWM_SERVO_2_GPIO GPIOA
+#define PWM_SERVO_2_PIN GPIO10
+#define PWM_SERVO_2_AF GPIO_AF1
+#define PWM_SERVO_2_OC TIM_OC3
+#define PWM_SERVO_2_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3 2
+#define PWM_SERVO_3_TIMER TIM8
+#define PWM_SERVO_3_GPIO GPIOC
+#define PWM_SERVO_3_PIN GPIO8
+#define PWM_SERVO_3_AF GPIO_AF3
+#define PWM_SERVO_3_OC TIM_OC3
+#define PWM_SERVO_3_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 3
+#define PWM_SERVO_4_TIMER TIM2
+#define PWM_SERVO_4_GPIO GPIOB
+#define PWM_SERVO_4_PIN GPIO11
+#define PWM_SERVO_4_AF GPIO_AF1
+#define PWM_SERVO_4_OC TIM_OC4
+#define PWM_SERVO_4_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 4
+#define PWM_SERVO_5_TIMER TIM3
+#define PWM_SERVO_5_GPIO GPIOB
+#define PWM_SERVO_5_PIN GPIO1
+#define PWM_SERVO_5_AF GPIO_AF2
+#define PWM_SERVO_5_OC TIM_OC4
+#define PWM_SERVO_5_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+#if USE_PWM6
+#define PWM_SERVO_6 5
+#define PWM_SERVO_6_TIMER TIM3
+#define PWM_SERVO_6_GPIO GPIOB
+#define PWM_SERVO_6_PIN GPIO0
+#define PWM_SERVO_6_AF GPIO_AF2
+#define PWM_SERVO_6_OC TIM_OC3
+#define PWM_SERVO_6_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_6_OC_BIT 0
+#endif
+
+/* servo 2 on TIM1 */
+#define PWM_TIM1_CHAN_MASK (PWM_SERVO_2_OC_BIT)
+/* servo 4 on TIM2 */
+#define PWM_TIM2_CHAN_MASK (PWM_SERVO_4_OC_BIT)
+/* servos 1,5,6 on TIM3 */
+#define PWM_TIM3_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT)
+/* servo 3 on TIM8 */
+#define PWM_TIM8_CHAN_MASK (PWM_SERVO_3_OC_BIT)
+
+/* by default activate onboard baro */
+#ifndef USE_BARO_BOARD
+#define USE_BARO_BOARD 1
+#endif
+
+#endif /* CONFIG_NAVSTIK_1_0_H */
diff --git a/sw/airborne/boards/px4fmu_1.7.h b/sw/airborne/boards/px4fmu_1.7.h
index 12472111d1..476879d822 100644
--- a/sw/airborne/boards/px4fmu_1.7.h
+++ b/sw/airborne/boards/px4fmu_1.7.h
@@ -226,7 +226,6 @@
#if USE_PWM1
#define PWM_SERVO_1 0
#define PWM_SERVO_1_TIMER TIM2
-#define PWM_SERVO_1_RCC RCC_GPIOA
#define PWM_SERVO_1_GPIO GPIOA
#define PWM_SERVO_1_PIN GPIO0
#define PWM_SERVO_1_AF GPIO_AF1
@@ -240,7 +239,6 @@
#if USE_PWM2
#define PWM_SERVO_2 1
#define PWM_SERVO_2_TIMER TIM2
-#define PWM_SERVO_2_RCC RCC_GPIOA
#define PWM_SERVO_2_GPIO GPIOA
#define PWM_SERVO_2_PIN GPIO1
#define PWM_SERVO_2_AF GPIO_AF1
@@ -254,7 +252,6 @@
#if USE_PWM3
#define PWM_SERVO_3_IDX 2
#define PWM_SERVO_3_TIMER TIM2
-#define PWM_SERVO_3_RCC RCC_GPIOA
#define PWM_SERVO_3_GPIO GPIOA
#define PWM_SERVO_3_PIN GPIO2
#define PWM_SERVO_3_AF GPIO_AF1
@@ -268,7 +265,6 @@
#if USE_PWM4
#define PWM_SERVO_4 3
#define PWM_SERVO_4_TIMER TIM2
-#define PWM_SERVO_4_RCC RCC_GPIOA
#define PWM_SERVO_4_GPIO GPIOA
#define PWM_SERVO_4_PIN GPIO3
#define PWM_SERVO_4_AF GPIO_AF1
diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h
index 8ca6d3c988..5935a0eacf 100644
--- a/sw/airborne/boards/stm32f4_discovery.h
+++ b/sw/airborne/boards/stm32f4_discovery.h
@@ -393,7 +393,6 @@
#if USE_PWM0
#define PWM_SERVO_0 0
#define PWM_SERVO_0_TIMER TIM1
-#define PWM_SERVO_0_RCC RCC_GPIOE
#define PWM_SERVO_0_GPIO GPIOE
#define PWM_SERVO_0_PIN GPIO9
#define PWM_SERVO_0_AF GPIO_AF1
@@ -406,7 +405,6 @@
#if USE_PWM1
#define PWM_SERVO_1 1
#define PWM_SERVO_1_TIMER TIM1
-#define PWM_SERVO_1_RCC RCC_GPIOE
#define PWM_SERVO_1_GPIO GPIOE
#define PWM_SERVO_1_PIN GPIO11
#define PWM_SERVO_1_AF GPIO_AF1
@@ -419,7 +417,6 @@
#if USE_PWM2
#define PWM_SERVO_2 2
#define PWM_SERVO_2_TIMER TIM1
-#define PWM_SERVO_2_RCC RCC_GPIOE
#define PWM_SERVO_2_GPIO GPIOE
#define PWM_SERVO_2_PIN GPIO13
#define PWM_SERVO_2_AF GPIO_AF1
@@ -432,7 +429,6 @@
#if USE_PWM3
#define PWM_SERVO_3 3
#define PWM_SERVO_3_TIMER TIM1
-#define PWM_SERVO_3_RCC RCC_GPIOE
#define PWM_SERVO_3_GPIO GPIOE
#define PWM_SERVO_3_PIN GPIO14
#define PWM_SERVO_3_AF GPIO_AF1
@@ -445,7 +441,6 @@
#if USE_PWM4
#define PWM_SERVO_4 4
#define PWM_SERVO_4_TIMER TIM9
-#define PWM_SERVO_4_RCC RCC_GPIOE
#define PWM_SERVO_4_GPIO GPIOE
#define PWM_SERVO_4_PIN GPIO5
#define PWM_SERVO_4_AF GPIO_AF3
@@ -458,7 +453,6 @@
#if USE_PWM5
#define PWM_SERVO_5 5
#define PWM_SERVO_5_TIMER TIM9
-#define PWM_SERVO_5_RCC RCC_GPIOE
#define PWM_SERVO_5_GPIO GPIOE
#define PWM_SERVO_5_PIN GPIO6
#define PWM_SERVO_5_AF GPIO_AF3
@@ -472,7 +466,6 @@
#if USE_PWM6
#define PWM_SERVO_6 6
#define PWM_SERVO_6_TIMER TIM5
-#define PWM_SERVO_6_RCC RCC_GPIOA
#define PWM_SERVO_6_GPIO GPIOA
#define PWM_SERVO_6_PIN GPIO3
#define PWM_SERVO_6_AF GPIO_AF2
@@ -485,7 +478,6 @@
#if USE_PWM7
#define PWM_SERVO_7 7
#define PWM_SERVO_7_TIMER TIM5
-#define PWM_SERVO_7_RCC RCC_GPIOA
#define PWM_SERVO_7_GPIO GPIOA
#define PWM_SERVO_7_PIN GPIO2
#define PWM_SERVO_7_AF GPIO_AF2
@@ -498,7 +490,6 @@
#if USE_PWM8
#define PWM_SERVO_8 8
#define PWM_SERVO_8_TIMER TIM5
-#define PWM_SERVO_8_RCC RCC_GPIOA
#define PWM_SERVO_8_GPIO GPIOA
#define PWM_SERVO_8_PIN GPIO1
#define PWM_SERVO_8_AF GPIO_AF2
@@ -511,7 +502,6 @@
#if USE_PWM9
#define PWM_SERVO_9 9
#define PWM_SERVO_9_TIMER TIM5
-#define PWM_SERVO_9_RCC RCC_GPIOA
#define PWM_SERVO_9_GPIO GPIOA
#define PWM_SERVO_9_PIN GPIO0
#define PWM_SERVO_9_AF GPIO_AF2
@@ -525,7 +515,6 @@
#if USE_PWM10
#define PWM_SERVO_10 10
#define PWM_SERVO_10_TIMER TIM12
-#define PWM_SERVO_10_RCC RCC_GPIOB
#define PWM_SERVO_10_GPIO GPIOB
#define PWM_SERVO_10_PIN GPIO14
#define PWM_SERVO_10_AF GPIO_AF9
@@ -538,7 +527,6 @@
#if USE_PWM11
#define PWM_SERVO_11 11
#define PWM_SERVO_11_TIMER TIM12
-#define PWM_SERVO_11_RCC RCC_GPIOB
#define PWM_SERVO_11_GPIO GPIOB
#define PWM_SERVO_11_PIN GPIO15
#define PWM_SERVO_11_AF GPIO_AF9
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
index de505dc46b..4c8155f2ee 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
@@ -57,16 +57,6 @@ extern float h_ctl_pitch_of_roll;
h_ctl_pitch_igain = _gain; \
}
-/* inner roll loop parameters */
-extern float h_ctl_ref_roll_angle;
-extern float h_ctl_ref_roll_rate;
-extern float h_ctl_ref_roll_accel;
-
-/* inner pitch loop parameters */
-extern float h_ctl_ref_pitch_angle;
-extern float h_ctl_ref_pitch_rate;
-extern float h_ctl_ref_pitch_accel;
-
extern bool_t use_airspeed_ratio;
#endif /* FW_H_CTL_A_H */
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index f92befc7b9..83d19a30be 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -460,7 +460,7 @@ void autopilot_check_in_flight(bool_t motors_on) {
void autopilot_set_motors_on(bool_t motors_on) {
- if (ahrs_is_aligned() && motors_on)
+ if (autopilot_mode != AP_MODE_KILL && ahrs_is_aligned() && motors_on)
autopilot_motors_on = TRUE;
else
autopilot_motors_on = FALSE;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index e0410d15c2..305fe08d70 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -446,7 +446,7 @@ static void guidance_h_traj_run(bool_t in_flight) {
((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* acceleration feedforward gain */
guidance_h_cmd_earth.y =
pd_y +
- ((guidance_h_vgain * guidance_h_speed_ref.x) >> 17) + /* speed feedforward gain */
+ ((guidance_h_vgain * guidance_h_speed_ref.y) >> 17) + /* speed feedforward gain */
((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* acceleration feedforward gain */
/* trim max bank angle from PD */
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 1692dbdb94..0a4807e620 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -70,15 +70,15 @@ float dist2_to_home;
bool_t too_far_from_home;
uint8_t horizontal_mode;
-uint8_t nav_segment_start, nav_segment_end;
-uint8_t nav_circle_centre;
+struct EnuCoor_i nav_segment_start, nav_segment_end;
+struct EnuCoor_i nav_circle_center;
int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
int32_t nav_leg_progress;
int32_t nav_leg_length;
int32_t nav_roll, nav_pitch;
-int32_t nav_heading, nav_course;
+int32_t nav_heading;
float nav_radius;
/** default nav_circle_radius in meters */
@@ -110,15 +110,15 @@ static void send_nav_status(void) {
&nav_block, &nav_stage,
&horizontal_mode);
if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
- float sx = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].x);
- float sy = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].y);
- float ex = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].x);
- float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y);
+ float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
+ float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
+ float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
+ float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, &sx, &sy, &ex, &ey);
}
else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
- float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x);
- float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y);
+ float cx = POS_FLOAT_OF_BFP(nav_circle_center.x);
+ float cy = POS_FLOAT_OF_BFP(nav_circle_center.y);
float r = POS_FLOAT_OF_BFP(nav_circle_radius);
DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, &cx, &cy, &r);
}
@@ -159,7 +159,6 @@ void nav_init(void) {
nav_roll = 0;
nav_pitch = 0;
nav_heading = 0;
- nav_course = 0;
nav_radius = DEFAULT_CIRCLE_RADIUS;
nav_throttle = 0;
nav_climb = 0;
@@ -210,13 +209,13 @@ void nav_run(void) {
nav_set_altitude();
}
-void nav_circle(uint8_t wp_center, int32_t radius) {
+void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
if (radius == 0) {
- VECT2_COPY(navigation_target, waypoints[wp_center]);
+ VECT2_COPY(navigation_target, *wp_center);
}
else {
struct Int32Vect2 pos_diff;
- VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_center]);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
// go back to half metric precision or values are too large
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
@@ -248,19 +247,18 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
// compute setpoint
VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
- VECT2_SUM(navigation_target, waypoints[wp_center], pos_diff);
+ VECT2_SUM(navigation_target, *wp_center, pos_diff);
}
- nav_circle_centre = wp_center;
+ nav_circle_center = *wp_center;
nav_circle_radius = radius;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
}
-//#include "stdio.h"
-void nav_route(uint8_t wp_start, uint8_t wp_end) {
+void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) {
struct Int32Vect2 wp_diff,pos_diff;
- VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
- VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), waypoints[wp_start]);
+ VECT2_DIFF(wp_diff, *wp_end, *wp_start);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
// go back to metric precision or values are too large
INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
@@ -269,29 +267,20 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress;
- int32_t prog_2 = nav_leg_length;// + progress / 2;
+ int32_t prog_2 = nav_leg_length;
Bound(nav_leg_progress, 0, prog_2);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
VECT2_SDIV(progress_pos, progress_pos, nav_leg_length);
- INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
- VECT2_SUM(navigation_target,waypoints[wp_start],progress_pos);
- //printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
- // navigation_target.x,
- // navigation_target.y,
- // progress_pos.x,
- // progress_pos.y,
- // waypoints[wp_start].x,
- // waypoints[wp_start].y,
- // leg_length, leg_length2, nav_leg_progress);
- //fflush(stdout);
+ INT32_VECT2_LSHIFT(progress_pos, progress_pos, INT32_POS_FRAC);
+ VECT2_SUM(navigation_target, *wp_start, progress_pos);
- nav_segment_start = wp_start;
- nav_segment_end = wp_end;
+ nav_segment_start = *wp_start;
+ nav_segment_end = *wp_end;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
}
-bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approaching_time) {
+bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time) {
int32_t dist_to_point;
struct Int32Vect2 diff;
struct EnuCoor_i* pos = stateGetPositionEnu_i();
@@ -304,11 +293,11 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approachin
VECT2_SMUL(estimated_progress, *speed, approaching_time);
INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
VECT2_SUM(estimated_pos, *pos, estimated_progress);
- VECT2_DIFF(diff, waypoints[wp_idx], estimated_pos);
+ VECT2_DIFF(diff, *wp, estimated_pos);
}
/* else use current position */
else {
- VECT2_DIFF(diff, waypoints[wp_idx], *pos);
+ VECT2_DIFF(diff, *wp, *pos);
}
/* compute distance of estimated/current pos to target wp
* distance with half metric precision (6.25 cm)
@@ -321,10 +310,10 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approachin
return TRUE;
/* if coming from a valid waypoint */
- if (from_idx > 0 && from_idx < NB_WAYPOINT) {
+ if (from != NULL) {
/* return TRUE if normal line at the end of the segment is crossed */
struct Int32Vect2 from_diff;
- VECT2_DIFF(from_diff, waypoints[wp_idx], waypoints[from_idx]);
+ VECT2_DIFF(from_diff, *wp, *from);
INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC/2);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
}
@@ -332,19 +321,19 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approachin
return FALSE;
}
-bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time) {
+bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
uint16_t time_at_wp;
int32_t dist_to_point;
static uint16_t wp_entry_time = 0;
static bool_t wp_reached = FALSE;
- static uint8_t wp_idx_last = 0;
+ static struct EnuCoor_i wp_last = { 0, 0, 0 };
struct Int32Vect2 diff;
- if (wp_idx_last != wp_idx) {
+ if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
wp_reached = FALSE;
- wp_idx_last = wp_idx;
+ wp_last = *wp;
}
- VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
+ VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
INT32_VECT2_NORM(dist_to_point, diff);
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2)){
@@ -361,7 +350,10 @@ bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time) {
time_at_wp = 0;
wp_reached = FALSE;
}
- if (time_at_wp > stay_time) return TRUE;
+ if (time_at_wp > stay_time) {
+ INT_VECT3_ZERO(wp_last);
+ return TRUE;
+ }
return FALSE;
}
@@ -508,3 +500,9 @@ bool_t nav_set_heading_towards(float x, float y) {
bool_t nav_set_heading_towards_waypoint(uint8_t wp) {
return nav_set_heading_towards(WaypointX(wp), WaypointY(wp));
}
+
+/** Set heading to the current yaw angle */
+bool_t nav_set_heading_current(void) {
+ nav_heading = stateGetNedToBodyEulers_i()->psi;
+ return FALSE;
+}
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index 1bbafb23c5..c37ff1d4c5 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -48,15 +48,15 @@ extern void nav_run(void);
extern uint8_t last_wp __attribute__ ((unused));
extern uint8_t horizontal_mode;
-extern uint8_t nav_segment_start, nav_segment_end;
-extern uint8_t nav_circle_centre;
+extern struct EnuCoor_i nav_segment_start, nav_segment_end;
+extern struct EnuCoor_i nav_circle_center;
extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
#define HORIZONTAL_MODE_WAYPOINT 0
#define HORIZONTAL_MODE_ROUTE 1
#define HORIZONTAL_MODE_CIRCLE 2
#define HORIZONTAL_MODE_ATTITUDE 3
extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC
-extern int32_t nav_heading, nav_course; ///< with #INT32_ANGLE_FRAC
+extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
extern float nav_radius;
extern int32_t nav_leg_progress;
@@ -89,6 +89,7 @@ extern bool_t nav_set_heading_rad(float rad);
extern bool_t nav_set_heading_deg(float deg);
extern bool_t nav_set_heading_towards(float x, float y);
extern bool_t nav_set_heading_towards_waypoint(uint8_t wp);
+extern bool_t nav_set_heading_current(void);
/** default approaching_time for a wp */
#ifndef CARROT
@@ -123,10 +124,10 @@ extern bool_t nav_set_heading_towards_waypoint(uint8_t wp);
}
/*********** Navigation on a circle **************************************/
-extern void nav_circle(uint8_t wp_center, int32_t radius);
+extern void nav_circle(struct EnuCoor_i * wp_center, int32_t radius);
#define NavCircleWaypoint(_center, _radius) { \
horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
- nav_circle(_center, POS_BFP_OF_REAL(_radius)); \
+ nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \
}
#define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
@@ -137,10 +138,10 @@ extern void nav_circle(uint8_t wp_center, int32_t radius);
#define NavCourseCloseTo(x) {}
/*********** Navigation along a line *************************************/
-extern void nav_route(uint8_t wp_start, uint8_t wp_end);
+extern void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end);
#define NavSegment(_start, _end) { \
horizontal_mode = HORIZONTAL_MODE_ROUTE; \
- nav_route(_start, _end); \
+ nav_route(&waypoints[_start], &waypoints[_end]); \
}
/** Nav glide routine */
@@ -152,13 +153,13 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end);
}
/** Proximity tests on approaching a wp */
-bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approaching_time);
-#define NavApproaching(wp, time) nav_approaching_from(wp, 0, time)
-#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from, time)
+bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time);
+#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp], NULL, time)
+#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp], &waypoints[from], time)
/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
-bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time);
-#define NavCheckWaypointTime(wp, time) nav_check_wp_time(wp, time)
+bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time);
+#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp], time)
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
diff --git a/sw/airborne/modules/gain_scheduling/gain_scheduling.h b/sw/airborne/modules/gain_scheduling/gain_scheduling.h
index 676491c035..4a5de10e1d 100644
--- a/sw/airborne/modules/gain_scheduling/gain_scheduling.h
+++ b/sw/airborne/modules/gain_scheduling/gain_scheduling.h
@@ -29,7 +29,7 @@
#define GAINS_SCHEDULING_H
#include "generated/airframe.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_int.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
extern struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS];
diff --git a/sw/airborne/modules/meteo/mf_ptu.c b/sw/airborne/modules/meteo/mf_ptu.c
new file mode 100644
index 0000000000..5a9d9d882a
--- /dev/null
+++ b/sw/airborne/modules/meteo/mf_ptu.c
@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "modules/meteo/mf_ptu.h"
+
+#include "mcu_periph/gpio.h"
+#include "mcu_periph/adc.h"
+#include "mcu_periph/pwm_input.h"
+#include "generated/airframe.h"
+
+/** Default scale and offset
+ * send raw values if nothing defined in airframe file
+ */
+#ifndef PTU_PRESSURE_OFFSET
+#define PTU_PRESSURE_OFFSET 0
+#endif
+#ifndef PTU_PRESSURE_SCALE
+#define PTU_PRESSURE_SCALE 1.0
+#endif
+#ifndef PTU_TEMPERATURE_OFFSET
+#define PTU_TEMPERATURE_OFFSET 0
+#endif
+#ifndef PTU_TEMPERATURE_SCALE
+#define PTU_TEMPERATURE_SCALE 1.0
+#endif
+#ifndef PTU_HUMIDTY_OFFSET
+#define PTU_HUMIDTY_OFFSET 0
+#endif
+#ifndef PTU_HUMIDTY_SCALE
+#define PTU_HUMIDTY_SCALE 1.0
+#endif
+
+/** ADC buffers */
+#ifndef ADC_CHANNEL_PTU_NB_SAMPLES
+#define ADC_CHANNEL_PTU_NB_SAMPLES DEFAULT_AV_NB_SAMPLE
+#endif
+struct adc_buf pressure_buf;
+struct adc_buf temp_buf;
+
+/** Raw values */
+uint16_t pressure_adc;
+uint16_t temp_adc;
+uint32_t humid_period;
+
+#if LOG_PTU
+#include "sdLog.h"
+#include "subsystems/chibios-libopencm3/chibios_sdlog.h"
+bool_t log_ptu_started;
+#endif
+
+#if SEND_PTU
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+#include "subsystems/gps.h"
+#endif
+
+void mf_ptu_init(void) {
+ adc_buf_channel(ADC_CHANNEL_PRESSURE, &pressure_buf, ADC_CHANNEL_PTU_NB_SAMPLES);
+ adc_buf_channel(ADC_CHANNEL_TEMPERATURE, &temp_buf, ADC_CHANNEL_PTU_NB_SAMPLES);
+
+#ifdef PTU_POWER_GPIO
+ gpio_setup_output(PTU_POWER_GPIO);
+ gpio_set(PTU_POWER_GPIO);
+#endif
+
+ pressure_adc = 0;
+ temp_adc = 0;
+ humid_period = 0;
+
+#if LOG_PTU
+ log_ptu_started = FALSE;
+#endif
+}
+
+void mf_ptu_periodic(void) {
+ // Read ADC
+ pressure_adc = pressure_buf.sum / pressure_buf.av_nb_sample;
+ temp_adc = temp_buf.sum / temp_buf.av_nb_sample;
+ // Read PWM
+ humid_period = USEC_OF_PWM_INPUT_TICKS(pwm_input_period_tics[PWM_INPUT_CHANNEL_HUMIDITY]);
+
+ // Log data
+#if LOG_PTU
+ if (pprzLogFile.fs != NULL) {
+ if (!log_ptu_started) {
+ sdLogWriteLog(&pprzLogFile, "P(adc) T(adc) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n");
+ log_ptu_started = TRUE;
+ }
+ else {
+ sdLogWriteLog(&pprzLogFile, "%d %d %d %d %d %d %d %d %d %d %d %d\n",
+ pressure_adc, temp_adc, humid_period,
+ gps.fix, gps.tow, gps.week,
+ gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
+ gps.gspeed, gps.course, -gps.ned_vel.z);
+ }
+ }
+#endif
+
+ // Send data
+#if SEND_PTU
+#define PTU_DATA_SIZE 3
+ float ptu_data[PTU_DATA_SIZE];
+ ptu_data[0] = (float)(PTU_PRESSURE_SCALE * ((int16_t)pressure_adc - PTU_PRESSURE_OFFSET));
+ ptu_data[1] = (float)(PTU_TEMPERATURE_SCALE * ((int16_t)temp_adc - PTU_TEMPERATURE_OFFSET));
+ ptu_data[2] = (float)(PTU_HUMIDTY_SCALE * ((int16_t)humid_period - PTU_HUMIDTY_OFFSET));
+ DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, PTU_DATA_SIZE, ptu_data);
+#endif
+}
+
+
diff --git a/sw/airborne/modules/meteo/mf_ptu.h b/sw/airborne/modules/meteo/mf_ptu.h
new file mode 100644
index 0000000000..d77974cca6
--- /dev/null
+++ b/sw/airborne/modules/meteo/mf_ptu.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** Data acquisition module for Meteo France PTU board
+ */
+
+#ifndef MF_PTU_H
+#define MF_PTU_H
+
+extern void mf_ptu_init(void);
+extern void mf_ptu_periodic(void);
+
+#endif
+
diff --git a/sw/airborne/modules/nav/nav_gls.c b/sw/airborne/modules/nav/nav_gls.c
index 6d725fab4d..dc2c42cd38 100644
--- a/sw/airborne/modules/nav/nav_gls.c
+++ b/sw/airborne/modules/nav/nav_gls.c
@@ -135,13 +135,11 @@ bool_t gls_start(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) {
init = TRUE;
-#if USE_AIRSPEED
//struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
//float wind_additional = sqrt(wind->x*wind->x + wind->y*wind->y); // should be gusts only!
//Bound(wind_additional, 0, 0.5);
//target_speed = STALL_AIRSPEED * 1.3 + wind_additional; FIXME
target_speed = APP_TARGET_SPEED; // ok for now!
-#endif
app_angle = APP_ANGLE;
app_intercept_rate = APP_INTERCEPT_RATE;
diff --git a/sw/airborne/peripherals/ms5611.c b/sw/airborne/peripherals/ms5611.c
index 9956d6583b..5b56b45c3c 100644
--- a/sw/airborne/peripherals/ms5611.c
+++ b/sw/airborne/peripherals/ms5611.c
@@ -58,8 +58,9 @@ bool_t ms5611_prom_crc_ok(uint16_t* prom) {
/**
* Calculate temperature and compensated pressure.
+ * @return TRUE if measurement was valid, FALSE otherwise
*/
-void ms5611_calc(struct Ms5611Data* ms) {
+bool_t ms5611_calc(struct Ms5611Data* ms) {
int64_t dt, tempms, off, sens, t2, off2, sens2;
/* difference between actual and ref temperature */
@@ -84,8 +85,14 @@ void ms5611_calc(struct Ms5611Data* ms) {
sens = sens - sens2;
}
- /* temperature in deg Celsius with 0.01 degC resolultion */
- ms->temperature = (int32_t)tempms;
/* temperature compensated pressure in Pascal (0.01mbar) */
- ms->pressure = (uint32_t)((((int64_t)ms->d1 * sens) / (1<<21) - off) / (1<<15));
+ uint32_t p = (((int64_t)ms->d1 * sens) / (1<<21) - off) / (1<<15);
+ /* if temp and pressare are in valid bounds, copy and return TRUE (valid) */
+ if ((tempms > -4000) && (tempms < 8500) && (p > 1000 ) && (p < 120000)) {
+ /* temperature in deg Celsius with 0.01 degC resolultion */
+ ms->temperature = (int32_t)tempms;
+ ms->pressure = p;
+ return TRUE;
+ }
+ return FALSE;
}
diff --git a/sw/airborne/peripherals/ms5611.h b/sw/airborne/peripherals/ms5611.h
index 85958d5ac5..0f7990a4a7 100644
--- a/sw/airborne/peripherals/ms5611.h
+++ b/sw/airborne/peripherals/ms5611.h
@@ -57,6 +57,6 @@ struct Ms5611Data {
};
extern bool_t ms5611_prom_crc_ok(uint16_t* prom);
-extern void ms5611_calc(struct Ms5611Data* ms);
+extern bool_t ms5611_calc(struct Ms5611Data* ms);
#endif /* MS5611_H */
diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c
index 1ec14b4611..0127c25f1c 100644
--- a/sw/airborne/peripherals/ms5611_i2c.c
+++ b/sw/airborne/peripherals/ms5611_i2c.c
@@ -151,10 +151,10 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) {
ms->status = MS5611_STATUS_IDLE;
}
else {
- /* calculate temp and pressure from measurements */
- ms5611_calc(&(ms->data));
+ /* calculate temp and pressure from measurements and set available if valid */
+ if (ms5611_calc(&(ms->data)))
+ ms->data_available = TRUE;
ms->status = MS5611_STATUS_IDLE;
- ms->data_available = TRUE;
}
break;
diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c
index d8e9293185..f015c34c7e 100644
--- a/sw/airborne/peripherals/ms5611_spi.c
+++ b/sw/airborne/peripherals/ms5611_spi.c
@@ -165,10 +165,10 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) {
ms->status = MS5611_STATUS_IDLE;
}
else {
- /* calculate temp and pressure from measurements */
- ms5611_calc(&(ms->data));
+ /* calculate temp and pressure from measurements and set available if valid */
+ if (ms5611_calc(&(ms->data)))
+ ms->data_available = TRUE;
ms->status = MS5611_STATUS_IDLE;
- ms->data_available = TRUE;
}
break;
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
index 20b9552518..b5cbb83579 100644
--- a/sw/airborne/subsystems/electrical.c
+++ b/sw/airborne/subsystems/electrical.c
@@ -52,8 +52,13 @@
#define ELECTRICAL_PERIODIC_FREQ 10
+#ifndef MIN_BAT_LEVEL
+#define MIN_BAT_LEVEL 3
+#endif
+
PRINT_CONFIG_VAR(LOW_BAT_LEVEL)
PRINT_CONFIG_VAR(CRITIC_BAT_LEVEL)
+PRINT_CONFIG_VAR(MIN_BAT_LEVEL)
struct Electrical electrical;
@@ -106,6 +111,7 @@ PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
void electrical_periodic(void) {
static uint8_t bat_low_counter = 0;
static uint8_t bat_critical_counter = 0;
+ static bool_t vsupply_check_started = FALSE;
#if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL)
electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample));
@@ -113,7 +119,8 @@ void electrical_periodic(void) {
#ifdef ADC_CHANNEL_CURRENT
#ifndef SITL
- electrical.current = MilliAmpereOfAdc((electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample));
+ int32_t current_adc = electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample;
+ electrical.current = MilliAmpereOfAdc(current_adc);
/* Prevent an overflow on high current spikes when using the motor brake */
BoundAbs(electrical.current, 65000);
#endif
@@ -139,28 +146,35 @@ void electrical_periodic(void) {
// mAh = mA * dt (10Hz -> hours)
electrical.energy += ((float)electrical.current) / 3600.0f / ELECTRICAL_PERIODIC_FREQ;
- if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
- if (bat_low_counter > 0)
- bat_low_counter--;
- if (bat_low_counter == 0)
- electrical.bat_low = TRUE;
- }
- else {
- // reset battery low status and counter
- bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
- electrical.bat_low = FALSE;
+ /*if valid voltage is seen then start checking. Set min level to 0 to always start*/
+ if (electrical.vsupply >= MIN_BAT_LEVEL * 10) {
+ vsupply_check_started = TRUE;
}
- if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
- if (bat_critical_counter > 0)
- bat_critical_counter--;
- if (bat_critical_counter == 0)
- electrical.bat_critical = TRUE;
- }
- else {
- // reset battery critical status and counter
- bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
- electrical.bat_critical = FALSE;
+ if (vsupply_check_started) {
+ if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
+ if (bat_low_counter > 0)
+ bat_low_counter--;
+ if (bat_low_counter == 0)
+ electrical.bat_low = TRUE;
+ }
+ else {
+ // reset battery low status and counter
+ bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
+ electrical.bat_low = FALSE;
+ }
+
+ if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
+ if (bat_critical_counter > 0)
+ bat_critical_counter--;
+ if (bat_critical_counter == 0)
+ electrical.bat_critical = TRUE;
+ }
+ else {
+ // reset battery critical status and counter
+ bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
+ electrical.bat_critical = FALSE;
+ }
}
}
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
index 85a1e22b8b..4e28480169 100644
--- a/sw/airborne/subsystems/imu.c
+++ b/sw/airborne/subsystems/imu.c
@@ -24,6 +24,7 @@
* Inertial Measurement Unit interface.
*/
+#include BOARD_CONFIG
#include "subsystems/imu.h"
#ifdef IMU_POWER_GPIO
diff --git a/sw/airborne/subsystems/imu/imu_navstik.c b/sw/airborne/subsystems/imu/imu_navstik.c
new file mode 100644
index 0000000000..3fcba91e40
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_navstik.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_navstik.c
+ * Driver for the Navstik magnetometer, accelerometer and gyroscope
+ */
+
+#include "subsystems/imu.h"
+#include "mcu_periph/i2c.h"
+
+
+/* defaults suitable for Navstik */
+#ifndef NAVSTIK_MAG_I2C_DEV
+#define NAVSTIK_MAG_I2C_DEV i2c3
+#endif
+PRINT_CONFIG_VAR(NAVSTIK_MAG_I2C_DEV)
+
+#ifndef NAVSTIK_MPU_I2C_DEV
+#define NAVSTIK_MPU_I2C_DEV i2c1
+#endif
+PRINT_CONFIG_VAR(NAVSTIK_MPU_I2C_DEV)
+
+#if !defined NAVSTIK_LOWPASS_FILTER && !defined NAVSTIK_SMPLRT_DIV
+#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
+/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
+ * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
+ */
+#define NAVSTIK_LOWPASS_FILTER MPU60X0_DLPF_42HZ
+#define NAVSTIK_SMPLRT_DIV 9
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
+#elif PERIODIC_FREQUENCY == 512
+/* Accelerometer: Bandwidth 260Hz, Delay 0ms
+ * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
+ */
+#define NAVSTIK_LOWPASS_FILTER MPU60X0_DLPF_256HZ
+#define NAVSTIK_SMPLRT_DIV 3
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
+#endif
+#endif
+PRINT_CONFIG_VAR(NAVSTIK_SMPLRT_DIV)
+PRINT_CONFIG_VAR(NAVSTIK_LOWPASS_FILTER)
+
+#ifndef NAVSTIK_GYRO_RANGE
+#define NAVSTIK_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
+#endif
+PRINT_CONFIG_VAR(NAVSTIK_GYRO_RANGE)
+
+#ifndef NAVSTIK_ACCEL_RANGE
+#define NAVSTIK_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
+#endif
+PRINT_CONFIG_VAR(NAVSTIK_ACCEL_RANGE)
+
+/** Basic Navstik IMU data */
+struct ImuNavstik imu_navstik;
+
+/**
+ * Navstik IMU initializtion of the MPU-60x0 and HMC58xx
+ */
+void imu_impl_init(void)
+{
+ imu_navstik.accel_valid = FALSE;
+ imu_navstik.gyro_valid = FALSE;
+ imu_navstik.mag_valid = FALSE;
+
+ /* MPU-60X0 */
+ mpu60x0_i2c_init(&imu_navstik.mpu, &(NAVSTIK_MPU_I2C_DEV), MPU60X0_ADDR_ALT);
+ imu_navstik.mpu.config.smplrt_div = NAVSTIK_SMPLRT_DIV;
+ imu_navstik.mpu.config.dlpf_cfg = NAVSTIK_LOWPASS_FILTER;
+ imu_navstik.mpu.config.gyro_range = NAVSTIK_GYRO_RANGE;
+ imu_navstik.mpu.config.accel_range = NAVSTIK_ACCEL_RANGE;
+
+ /* HMC58XX */
+ hmc58xx_init(&imu_navstik.hmc, &(NAVSTIK_MAG_I2C_DEV), HMC58XX_ADDR);
+}
+
+/**
+ * Handle all the periodic tasks of the Navstik IMU components.
+ * Read the MPU60x0 every periodic call and the HMC58XX every 10th call.
+ */
+void imu_periodic(void)
+{
+ // Start reading the latest gyroscope data
+ mpu60x0_i2c_periodic(&imu_navstik.mpu);
+
+ // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
+ RunOnceEvery(10, hmc58xx_periodic(&imu_navstik.hmc));
+}
+
+/**
+ * Handle all the events of the Navstik IMU components.
+ * When there is data available convert it to the correct axis and save it in the imu structure.
+ */
+void imu_navstik_event(void)
+{
+ /* MPU-60x0 event taks */
+ mpu60x0_i2c_event(&imu_navstik.mpu);
+
+ if (imu_navstik.mpu.data_available) {
+ /* default orientation as should be printed on the pcb, z-down, ICs down */
+ RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
+ VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);
+
+ imu_navstik.mpu.data_available = FALSE;
+ imu_navstik.gyro_valid = TRUE;
+ imu_navstik.accel_valid = TRUE;
+ }
+
+ /* HMC58XX event task */
+ hmc58xx_event(&imu_navstik.hmc);
+ if (imu_navstik.hmc.data_available) {
+ imu.mag_unscaled.x = imu_navstik.hmc.data.vect.y;
+ imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
+ imu.mag_unscaled.z = imu_navstik.hmc.data.vect.z;
+
+ imu_navstik.hmc.data_available = FALSE;
+ imu_navstik.mag_valid = TRUE;
+ }
+}
diff --git a/sw/airborne/subsystems/imu/imu_navstik.h b/sw/airborne/subsystems/imu/imu_navstik.h
new file mode 100644
index 0000000000..88c2901bb8
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_navstik.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_navstik.h
+ * Interface for the Navstik magnetometer, accelerometer and gyroscope
+ */
+
+
+#ifndef IMU_NAVSTIK_H
+#define IMU_NAVSTIK_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "peripherals/hmc58xx.h"
+#include "peripherals/mpu60x0_i2c.h"
+
+/* Defaults */
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+
+/** default gyro sensitivy and neutral from the datasheet
+ * MPU with 1000 deg/s has 32.8 LSB/(deg/s)
+ * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/32.8 * pi/180 * 4096 = 2.17953
+ I*/
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+// FIXME
+#define IMU_GYRO_P_SENS 2.17953
+#define IMU_GYRO_P_SENS_NUM 18271
+#define IMU_GYRO_P_SENS_DEN 8383
+#define IMU_GYRO_Q_SENS 2.17953
+#define IMU_GYRO_Q_SENS_NUM 18271
+#define IMU_GYRO_Q_SENS_DEN 8383
+#define IMU_GYRO_R_SENS 2.17953
+#define IMU_GYRO_R_SENS_NUM 18271
+#define IMU_GYRO_R_SENS_DEN 8383
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+/** default accel sensitivy from the datasheet
+ * MPU with 8g has 4096 LSB/g
+ * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+// FIXME
+#define IMU_ACCEL_X_SENS 2.4525
+#define IMU_ACCEL_X_SENS_NUM 981
+#define IMU_ACCEL_X_SENS_DEN 400
+#define IMU_ACCEL_Y_SENS 2.4525
+#define IMU_ACCEL_Y_SENS_NUM 981
+#define IMU_ACCEL_Y_SENS_DEN 400
+#define IMU_ACCEL_Z_SENS 2.4525
+#define IMU_ACCEL_Z_SENS_NUM 981
+#define IMU_ACCEL_Z_SENS_DEN 400
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
+
+struct ImuNavstik {
+ volatile uint8_t accel_valid;
+ volatile uint8_t gyro_valid;
+ volatile uint8_t mag_valid;
+ struct Mpu60x0_I2c mpu;
+ struct Hmc58xx hmc;
+};
+
+extern struct ImuNavstik imu_navstik;
+extern void imu_navstik_event(void);
+
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ imu_navstik_event();
+ if (imu_navstik.gyro_valid) {
+ imu_navstik.gyro_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_navstik.accel_valid) {
+ imu_navstik.accel_valid = FALSE;
+ _accel_handler();
+ }
+ if (imu_navstik.mag_valid) {
+ imu_navstik.mag_valid = FALSE;
+ _mag_handler();
+ }
+}
+
+#endif /* IMU_NAVSTIK_H */
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
index eab68e3964..c9ab69a884 100644
--- a/sw/airborne/subsystems/ins/ins_float_invariant.c
+++ b/sw/airborne/subsystems/ins/ins_float_invariant.c
@@ -194,6 +194,9 @@ PRINT_CONFIG_VAR(INS_BARO_ID)
abi_event baro_ev;
static void baro_cb(uint8_t sender_id, const float *pressure);
+/* gps */
+bool_t ins_gps_fix_once;
+
/* error computation */
static inline void error_output(struct InsFloatInv * _ins);
@@ -217,6 +220,7 @@ static inline void init_invariant_state(void) {
// init baro
ins_baro_initialized = FALSE;
+ ins_gps_fix_once = FALSE;
}
void ins_init() {
@@ -465,6 +469,7 @@ void ahrs_propagate(void) {
void ahrs_update_gps(void) {
if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) {
+ ins_gps_fix_once = TRUE;
#if INS_UPDATE_FW_ESTIMATOR
if (state.utm_initialized_f) {
@@ -618,13 +623,14 @@ static inline void error_output(struct InsFloatInv * _ins) {
FLOAT_VECT3_DIFF(Eb, B, YBt);
// pos and speed error only if GPS data are valid
- if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING
+ // or while waiting first GPS data to prevent diverging
+ if ((gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING
#if INS_UPDATE_FW_ESTIMATOR
&& state.utm_initialized_f
#else
&& state.ned_initialized_f
#endif
- ) {
+ ) || !ins_gps_fix_once) {
/* Ev = (V - YV) */
FLOAT_VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
/* Ex = (X - YX) */
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 749cdb8a95..74f2a5b054 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -80,19 +80,21 @@ static void sonar_cb(uint8_t sender_id, const float *distance);
#define INS_SONAR_MIN_RANGE 0.001
#endif
#define VFF_R_SONAR_0 0.1
+#ifndef VFF_R_SONAR_OF_M
#define VFF_R_SONAR_OF_M 0.2
+#endif
#ifndef INS_SONAR_UPDATE_ON_AGL
#define INS_SONAR_UPDATE_ON_AGL FALSE
PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
#endif
+#endif // USE_SONAR
+
#ifndef INS_VFF_R_GPS
#define INS_VFF_R_GPS 2.0
#endif
-#endif // USE_SONAR
-
#ifndef USE_INS_NAV_INIT
#define USE_INS_NAV_INIT TRUE
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
diff --git a/sw/airborne/test/mcu_periph/test_adc.c b/sw/airborne/test/mcu_periph/test_adc.c
index 86cd099b7c..09f31c1291 100644
--- a/sw/airborne/test/mcu_periph/test_adc.c
+++ b/sw/airborne/test/mcu_periph/test_adc.c
@@ -37,8 +37,11 @@ static struct adc_buf adc0_buf;
static struct adc_buf adc1_buf;
static struct adc_buf adc2_buf;
static struct adc_buf adc3_buf;
+static struct adc_buf vsupply_buf;
-extern uint8_t adc_new_data_trigger;
+#ifndef VoltageOfAdc
+#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
+#endif
static inline void main_init( void ) {
mcu_init();
@@ -48,6 +51,7 @@ static inline void main_init( void ) {
adc_buf_channel(1, &adc1_buf, 3);
adc_buf_channel(2, &adc2_buf, 3);
adc_buf_channel(3, &adc3_buf, 3);
+ adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_buf, DEFAULT_AV_NB_SAMPLE);
}
int main( void ) {
@@ -66,18 +70,12 @@ static inline void main_periodic_task( void ) {
RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
RunOnceEvery(100, {DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sys_time.nb_sec);});
LED_PERIODIC();
+
+ uint16_t v1 = 10 * VoltageOfAdc((vsupply_buf.sum/vsupply_buf.av_nb_sample));
+ uint16_t v2 = 10 * VoltageOfAdc((vsupply_buf.values[0]));
+ RunOnceEvery(50, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2)});
}
static inline void main_event_task( void ) {
- if (adc_new_data_trigger) {
- adc_new_data_trigger = 0;
- uint16_t v1 = 123;
- uint16_t v2 = 123;
- // v1 = (((adc0_buf.values[0])));
- v1 = adc0_buf.sum/adc0_buf.av_nb_sample;
- v2 = (((adc3_buf.values[0])));
- RunOnceEvery(100, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2)});
- }
-
}
diff --git a/sw/airborne/test/mcu_periph/test_sys_time_timer.c b/sw/airborne/test/mcu_periph/test_sys_time_timer.c
index 46d721baf2..65cf01a76f 100644
--- a/sw/airborne/test/mcu_periph/test_sys_time_timer.c
+++ b/sw/airborne/test/mcu_periph/test_sys_time_timer.c
@@ -68,7 +68,7 @@ static inline void main_periodic_03( void ) {
/*
Called from the systime interrupt handler
*/
-static inline void main_periodic_05( uint8_t id ) {
+static inline void main_periodic_05( __attribute__((unused)) uint8_t id ) {
#ifdef LED_RED
LED_TOGGLE(LED_RED);
#endif
diff --git a/sw/ext/ardrone2_drivers/ardrone2.py b/sw/ext/ardrone2_drivers/ardrone2.py
index 0bbae6d52d..fd6259188c 100755
--- a/sw/ext/ardrone2_drivers/ardrone2.py
+++ b/sw/ext/ardrone2_drivers/ardrone2.py
@@ -5,6 +5,7 @@ import re
import argparse
import socket
import telnetlib
+import os
from time import sleep
from ftplib import FTP
@@ -82,6 +83,14 @@ def check_autoboot():
else:
return False
+# Check if custom wifi_setup script is installed
+def check_wifi_setup():
+ check_wifi = execute_command('grep "static_ip_address_base" /bin/wifi_setup.sh')
+ if "static_ip_address_base" in check_wifi:
+ return True
+ else:
+ return False
+
# Check the filesystem
def check_filesystem():
return execute_command('df -h')
@@ -122,6 +131,42 @@ def ardrone2_start_vision():
# Show result
execute_command("ls -altr /opt/arm/gst/bin")
+# Install autoboot script
+def ardrone2_install_autoboot():
+ print('Uploading autoboot script')
+ ftp.storbinary("STOR check_update.sh", file("check_update.sh", "rb"))
+ print(execute_command("mv /data/video/check_update.sh /bin/check_update.sh"))
+ print(execute_command("chmod 777 /bin/check_update.sh"))
+
+# Install network script
+def ardrone2_install_network_script():
+ print('Uploading Wifi script')
+ ftp.storbinary("STOR wifi_setup.sh", file("wifi_setup.sh", "rb"))
+ print(execute_command("mv /data/video/wifi_setup.sh /bin/wifi_setup.sh"))
+ print(execute_command("chmod 777 /bin/wifi_setup.sh"))
+
+# Set network SSID
+def ardrone2_set_ssid(name):
+ write_to_config('ssid_single_player', name)
+ print('The network ID (SSID) of the ARDrone 2 is changed to ' + name)
+
+# Set IP address
+def ardrone2_set_ip_address(address):
+ splitted_ip = address.split(".")
+ write_to_config('static_ip_address_base', splitted_ip[0] + '.' + splitted_ip[1] + '.' + splitted_ip[2] + '.')
+ write_to_config('static_ip_address_probe', splitted_ip[3])
+ print('The IP Address of the ARDrone 2 is changed to ' + address)
+
+# Set wifi mode (0: master, 1: ad-hoc, 2: managed, *: master)
+def ardrone2_set_wifi_mode(mode):
+ modes = { 'master' : '0', 'ad-hoc' : '1', 'managed' : '2' }
+ try:
+ val = modes[mode]
+ except:
+ print('Unexpected wifi mode, setting to master (default)')
+ val = modes['master']
+ write_to_config('wifi_mode', val)
+ print('The Wifi mode of the ARDrone2 is changed to ' + mode + ' (' + val + ')')
def ardrone2_status():
config_ini = execute_command('cat /data/config.ini')
@@ -150,7 +195,7 @@ def ardrone2_status():
vision_framework = ""
if check_vision_installed():
vision_framework += "Installed"
- if check_vision_running:
+ if check_vision_running():
vision_framework += " and running"
print('Vision framework:\t' + vision_framework)
@@ -169,12 +214,18 @@ subparsers = parser.add_subparsers(title='Command to execute', metavar='command'
subparsers.add_parser('status', help='Request the status of the ARDrone 2')
subparsers.add_parser('reboot', help='Reboot the ARDrone 2')
subparsers.add_parser('installvision', help='Install the vision framework')
-subparser_upload = subparsers.add_parser('upload_gst_module',
+subparser_upload_gst = subparsers.add_parser('upload_gst_module',
help='Upload, configure and move a gstreamer0.10 module libXXX.so')
-subparser_upload.add_argument('file', help='Filename of *.so module')
-subparser_paparazzi = subparsers.add_parser('upload_paparazzi', help='Upload paparazzi autopilot software')
-subparser_paparazzi.add_argument('file', help='Filename of *.elf executable')
-subparser_paparazzi.add_argument('folder', help='Destination Subfolder: raw or sdk')
+subparser_upload_gst.add_argument('file', help='Filename of *.so module')
+subparser_upload_and_run = subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)')
+subparser_upload_and_run.add_argument('file', help='Filename of an executable')
+subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)')
+subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the ARDrone 2')
+subparser_upload.add_argument('file', help='Filename')
+subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)')
+subparser_download = subparsers.add_parser('download_file', help='Download a file from the ARDrone 2')
+subparser_download.add_argument('file', help='Filename (with the path on the local machine)')
+subparser_download.add_argument('folder', help='Remote subfolder (base folder is /data/video)')
subparser_insmod = subparsers.add_parser('insmod', help='Upload and insert kernel module')
subparser_insmod.add_argument('file', help='Filename of *.ko kernel module')
subparsers.add_parser('startvision', help='Start the vision framework')
@@ -186,6 +237,15 @@ subparser_networkid = subparsers.add_parser('networkid', help='Set the network I
subparser_networkid.add_argument('name', help='the new network ID(SSID)')
subparser_ipaddress = subparsers.add_parser('ipaddress', help='Set the IP address of the ARDrone 2')
subparser_ipaddress.add_argument('address', help='the new IP address')
+subparser_wifimode = subparsers.add_parser('wifimode', help='Set the Wifi mode the ARDrone 2')
+subparser_wifimode.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed'])
+subparser_configure_network = subparsers.add_parser('configure_network', help='Configure the network on the ARDrone 2')
+subparser_configure_network.add_argument('name', help='the new network ID(SSID)')
+subparser_configure_network.add_argument('address', help='the new IP address')
+subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed'])
+subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the ARDrone 2')
+subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
+ help='what to start on boot')
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the ARDrone 2')
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
help='what to start on boot')
@@ -225,20 +285,68 @@ elif args.command == 'start':
# Change the network ID
elif args.command == 'networkid':
- write_to_config('ssid_single_player', args.name)
- print('The network ID (SSID) of the ARDrone 2 is changed to ' + args.name)
+ ardrone2_set_ssid(args.name)
- if input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
+ if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
ardrone2_reboot()
# Change the IP address
elif args.command == 'ipaddress':
- splitted_ip = args.address.split(".")
- write_to_config('static_ip_address_base', splitted_ip[0] + '.' + splitted_ip[1] + '.' + splitted_ip[2] + '.')
- write_to_config('static_ip_address_probe', splitted_ip[3])
- print('The IP Address of the ARDrone 2 is changed to ' + args.address)
+ ardrone2_set_ip_address(args.address)
- if input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
+ if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
+ ardrone2_reboot()
+
+# Change the wifi mode
+elif args.command == 'wifimode':
+ ardrone2_set_wifi_mode(args.mode)
+
+ if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
+ ardrone2_reboot()
+
+# Install and configure network
+elif args.command == 'configure_network':
+ config_ini = execute_command('cat /data/config.ini')
+ print('=== Current network setup ===')
+ print('Network id:\t' + read_from_config('ssid_single_player', config_ini))
+ print('Host:\t\t' + args.host + ' (' + read_from_config('static_ip_address_base', config_ini) +
+ read_from_config('static_ip_address_probe', config_ini) + ' after boot)')
+ print('Mode:\t\t' + read_from_config('wifi_mode', config_ini))
+ print('=============================')
+ if check_wifi_setup():
+ print('Custom Wifi script already installed')
+ if raw_input("Shall I reinstall the Wifi script (y/N) ").lower() == 'y':
+ ardrone2_install_network_script()
+ else:
+ if raw_input("Shall I install custom Wifi script (recommanded) (y/N) ").lower() == 'y':
+ ardrone2_install_network_script()
+ ardrone2_set_ssid(args.name)
+ ardrone2_set_ip_address(args.address)
+ ardrone2_set_wifi_mode(args.mode)
+ config_ini = execute_command('cat /data/config.ini')
+ print('== New network setup after boot ==')
+ print('Network id:\t' + read_from_config('ssid_single_player', config_ini))
+ print('Host:\t\t' + read_from_config('static_ip_address_base', config_ini) +
+ read_from_config('static_ip_address_probe', config_ini))
+ print('Mode:\t\t' + read_from_config('wifi_mode', config_ini))
+ print('==================================')
+
+ if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
+ ardrone2_reboot()
+
+# Install and configure autostart
+elif args.command == 'install_autostart':
+ if check_autoboot():
+ print('Custom autostart script already installed')
+ if raw_input("Shall I reinstall the autostart script (y/N) ").lower() == 'y':
+ ardrone2_install_autoboot()
+ else:
+ ardrone2_install_autoboot()
+ autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'}
+ write_to_config('start_paparazzi', autorun[args.type])
+ print('The autostart on boot is changed to ' + args.type)
+
+ if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
ardrone2_reboot()
# Change the autostart
@@ -251,7 +359,7 @@ elif args.command == 'autostart':
elif args.command == 'installvision':
if check_vision_installed():
print('Vision framework already installed')
- if input("Shall I reinstall the vision framework? (y/N) ").lower() == 'y':
+ if raw_input("Shall I reinstall the vision framework? (y/N) ").lower() == 'y':
ardrone2_remove_vision()
ardrone2_install_vision()
@@ -265,7 +373,7 @@ elif args.command == 'startvision':
else:
if not check_vision_installed():
print('No vision framework installed')
- if input("Shall I install the vision framework? (y/N) ").lower() == 'y':
+ if raw_input("Shall I install the vision framework? (y/N) ").lower() == 'y':
ardrone2_install_vision()
if check_vision_installed():
@@ -282,7 +390,7 @@ elif args.command == 'upload_gst_module':
else:
if not check_vision_installed():
print('Warning: No vision framework installed')
- if input("Warning: Shall I install the vision framework? (y/N) ").lower() == 'y':
+ if raw_input("Warning: Shall I install the vision framework? (y/N) ").lower() == 'y':
ardrone2_install_vision()
if check_vision_installed():
@@ -297,7 +405,7 @@ elif args.command == 'insmod':
ftp.storbinary("STOR " + modfile[1], file(args.file, "rb"))
print(execute_command("insmod /data/video/" + modfile[1]))
-elif args.command == 'upload_paparazzi':
+elif args.command == 'upload_file_and_run':
# Split filename and path
f = split_into_path_and_file(args.file)
@@ -312,6 +420,32 @@ elif args.command == 'upload_paparazzi':
execute_command("/data/video/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &")
print("#pragma message: Upload and Start of ap.elf to ARDrone2 Succes!")
+elif args.command == 'upload_file':
+ # Split filename and path
+ f = split_into_path_and_file(args.file)
+
+ execute_command("mkdir -p /data/video/" + args.folder)
+ print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/video/" + args.folder)
+ ftp.storbinary("STOR " + args.folder + "/" + f[1], file(args.file, "rb"))
+ print("#pragma message: Upload of " + f[1] + " to ARDrone2 Succes!")
+
+elif args.command == 'download_file':
+ # Split filename and path
+ f = split_into_path_and_file(args.file)
+ # Open file and download
+ try:
+ file = open(args.file, 'wb')
+ print('Downloading \'' + f[1] + "\' from " + args.folder + " to " + f[0])
+ ftp.retrbinary("RETR " + args.folder + "/" + f[1], file.write)
+ print("#pragma message: Download of " + f[1] + " from ARDrone2 Succes!")
+ except IOError:
+ print("#pragma message: Fail to open file" + args.file)
+ except:
+ os.remove(args.file)
+ print("#pragma message: Download of " + f[1] + " from ARDrone2 Failed!")
+ else:
+ file.close()
+
# Close the telnet and python script
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index dacb0ad9d4..e43e8ba78c 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -322,7 +322,7 @@ let mark = fun (geomap:G.widget) ac_id track plugin_frame ->
(** Light display of attributes in the flight plan. *)
let attributes_pretty_printer = fun attribs ->
- (* Remove the optional attributes *)
+ (* Remove the optional attributes *)
let valid = fun a ->
let a = String.lowercase a in
a <> "no" && a <> "strip_icon" && a <> "strip_button" && a <> "pre_call"
@@ -1252,7 +1252,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
match ap_mode with
"AUTO2" | "NAV" -> ok_color
| "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" -> "#10F0E0"
- | "MANUAL" | "RATE" | "ATT" | "RC_D" -> warning_color
+ | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color
| _ -> alert_color in
ac.strip#set_color "AP" color;
end;
diff --git a/sw/ground_segment/tmtc/app_server.c b/sw/ground_segment/tmtc/app_server.c
index f4ed772eee..502b970018 100644
--- a/sw/ground_segment/tmtc/app_server.c
+++ b/sw/ground_segment/tmtc/app_server.c
@@ -41,8 +41,6 @@
#include
#include
-// assuming local path from sw/ground_segment/tmtc
-char defaultPprzFolder[] = "../../..";
char defaultAppPass[] = "1234"; //4 char password to control ac's over app "pass ground stg stg stg..
char* AppPass;
@@ -74,6 +72,12 @@ char ivybuffer[BUFLEN];
// verbose flag
int verbose = 0;
+//TCP flag
+int uTCP = 0;
+
+int ProcessID;
+int RequestID;
+
//Block structure
typedef struct {
//Will we need any more block data?
@@ -107,8 +111,9 @@ device_names DevNames[MAXDEVICENUMB];
//Connected Clients Data Structure
typedef struct {
int used ;
- int client_port; //do we need that??
char client_ip[MAXIPLEN];
+ //Pointer for tcp connection;
+ gpointer ClientTcpData;
} client_data;
client_data ConnectedClients[MAXCLIENT]; //Holds all status of devices
@@ -143,7 +148,7 @@ void remove_client(char* RemClientIpAd) {
}
//Record client (if new)
-void add_client(char* ClientIpAd) {
+void add_client(char* ClientIpAd, gpointer connection_in) {
/* Check if client exists. If exists return else record it */
int i;
for (i = 0; i < MAXCLIENT; i++) {
@@ -160,6 +165,7 @@ void add_client(char* ClientIpAd) {
//record new client ip
g_stpcpy(ConnectedClients[i].client_ip,ClientIpAd);
+ ConnectedClients[i].ClientTcpData = connection_in;
//
ConnectedClients[i].used = 1;
if (verbose) {
@@ -191,7 +197,8 @@ int get_ac_data(char* InStr, char* RetBuf) {
DevNames[AcID].color,
DevNames[AcID].dl_launch_ind,
DevNames[AcID].kill_thr_ind,
- DevNames[AcID].flight_altitude_ind);
+ DevNames[AcID].flight_altitude_ind
+ );
}
return AcID;
}
@@ -255,6 +262,22 @@ int get_bl_data(char* InStr, char* RetBuf) {
void broadcast_to_clients () {
int i;
+
+ if (uTCP) {
+ //broadcast using tcp connection
+ GError *error = NULL;
+
+ for (i = 0; i < MAXCLIENT; i++) {
+ if (ConnectedClients[i].used > 0) {
+ GOutputStream * ostream = g_io_stream_get_output_stream (ConnectedClients[i].ClientTcpData);
+ g_output_stream_write(ostream, ivybuffer, strlen(ivybuffer), NULL, &error);
+ }
+
+ }
+ return;
+ }
+
+ i=0;
for (i = 0; i < MAXCLIENT; i++) {
if (ConnectedClients[i].used > 0) {
@@ -287,6 +310,7 @@ void broadcast_to_clients () {
g_object_unref(udpSocket);
}
}
+
}
//Read tcp requests of connected clients
@@ -298,8 +322,21 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) {
if (ret == G_IO_STATUS_ERROR) {
//unref connection
- g_object_unref (data);
- g_string_free (s,TRUE);
+ GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL);
+ GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr));
+ //Read sender ip
+ if (verbose) {
+ printf("App Server: Communication error.. Removing client.. ->%s\n", g_inet_address_to_string(addr));
+ fflush(stdout);
+ }
+ //Remove client
+ remove_client(g_inet_address_to_string(addr));
+ //Free objects
+ g_string_free(s, TRUE);
+ g_object_unref(sockaddr);
+ g_object_unref(addr);
+ g_object_unref(data);
+ //Return false to stop listening this socket
return FALSE;
}
else{
@@ -313,7 +350,7 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) {
incs = g_string_erase(s,0,(strlen(AppPass)+1));
IvySendMsg("%s",incs->str);
if (verbose) {
- printf("App Server: Command passed to ivy.. %s\n",incs->str);
+ printf("App Server: Command passed to ivy: %s\n",incs->str);
fflush(stdout);
}
}
@@ -371,7 +408,7 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) {
else {
//Unknown command
if (verbose) {
- printf("App Server: Client send an unknown command or wrong password: %s\n",RecString);
+ printf("App Server: Client send an unknown command or wrong password: (%s)\n",RecString);
fflush(stdout);
}
}
@@ -380,7 +417,10 @@ gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) {
if (ret == G_IO_STATUS_EOF) {
//Client disconnected
if (verbose) {
- printf("App Server: Client disconnected without saying 'bye':(\n");
+ GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL);
+ GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr));
+ printf("App Server: Client disconnected without saying 'bye':( ->%s\n", g_inet_address_to_string(addr));
+ remove_client(g_inet_address_to_string(addr));
fflush(stdout);
}
g_string_free(s, TRUE);
@@ -407,7 +447,7 @@ gboolean new_connection(GSocketService *service, GSocketConnection *connection,
}
//Record client (if new)
- add_client(g_inet_address_to_string(addr));
+ add_client(g_inet_address_to_string(addr), connection);
g_object_ref (connection);
GSocket *socket = g_socket_connection_get_socket(connection);
@@ -418,11 +458,28 @@ gboolean new_connection(GSocketService *service, GSocketConnection *connection,
return TRUE;
}
+void request_ac_config(int ac_id_req) {
+
+ RequestID++;
+ IvySendMsg("app_server %d_%d CONFIG_REQ %d" ,ProcessID, RequestID ,ac_id_req );
+ //AC config requested..
+
+ if (verbose) {
+ printf("AC(id= %d) config requested.\n",ac_id_req);
+ fflush(stdout);
+ }
+}
+
//Ivy msg function
void Ivy_All_Msgs(IvyClientPtr app, void *user_data, int argc, char *argv[]){
+
+ //For compatibility.. This will be joined in upcoming releases..
+ if (uTCP) sprintf(ivybuffer, "%s\n", argv[0]);
+ else sprintf(ivybuffer, "%s", argv[0]);
+
//Ivy msg received broadcast to clients..
- sprintf(ivybuffer, "%s", argv[0]);
broadcast_to_clients();
+
}
//Parse AC flight plan xml (block & waypoint names
@@ -551,7 +608,7 @@ void parse_ac_af(int DevNameIndex, char *filename) {
}
//Parse dl values
-void parse_dl_settings(int DevNameIndex, char *filename) {
+void parse_ac_settings(int DevNameIndex, char *filename) {
xmlTextReaderPtr reader;
int ret;
@@ -608,106 +665,136 @@ void parse_dl_settings(int DevNameIndex, char *filename) {
}
}
-//Parse ac data from conf.xml
-void parse_ac_data (char *PprzFolder) {
- xmlTextReaderPtr reader;
- int ret;
+void on_app_server_NEW_AC(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
+ //Request config
+ request_ac_config(atoi(argv[0]));
+}
- //Create full file path
- char xmlFileName[BUFLEN];
- strcpy(xmlFileName, PprzFolder);
- strcat(xmlFileName, "/conf/conf.xml");
+void on_app_server_GET_CONFIG (IvyClientPtr app, void *user_data, int argc, char *argv[]) {
- reader = xmlReaderForFile(xmlFileName, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */
+ int i=0;
- xmlChar *AcName, *AcInd, *FpPath, *AcColor, *name, *AfPath;
- if (reader != NULL) {
- ret = xmlTextReaderRead(reader);
- int AcId;
+ int RmId[2];
+ /*
+ * RmId[0] >> ProcessID of incoming MsgProcessId
+ * RmId[1] >> RequestID of incoming MsgProcessId
+ */
- while (ret == 1) {
- name = xmlTextReaderName(reader);
- if (name == NULL) {
- name = xmlStrdup(BAD_CAST "--");
- }
- //read waypoint names
-
- if (xmlStrEqual(name, (const xmlChar *)"aircraft")) {
-
- xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"ac_id");
- AcInd = xmlTextReaderValue(reader);
-
- xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name");
- AcName = xmlTextReaderValue(reader);
-
- xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"airframe");
- AfPath = xmlTextReaderValue(reader);
-
- xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"flight_plan");
- FpPath = xmlTextReaderValue(reader);
-
- xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"gui_color");
- AcColor = xmlTextReaderValue(reader);
-
- //Get Device Id
- AcId = atoi(((char *) AcInd));
-
- //Save Device Name
- strcpy(DevNames[AcId].name, ((char *) AcName));
-
- //Save color
- strcpy(DevNames[AcId].color, (char *) AcColor);
-
- //Save Flight Plan Path
- strcpy(DevNames[AcId].flight_plan_path, "/conf/");
- strcat(DevNames[AcId].flight_plan_path , (char *) FpPath);
-
- //Save airframe Path
- strcpy(DevNames[AcId].airframe_path, "/conf/");
- strcat(DevNames[AcId].airframe_path , (char *) AfPath);
-
- //Save Settings Path
- sprintf(DevNames[AcId].settings_path, "/var/%s/settings.xml", (char *) AcName);
-
- //parse flight plan file for waypoint and block names
- char FlightPlanPath[BUFLEN];
- strcpy(FlightPlanPath, PprzFolder);
- strcat(FlightPlanPath, DevNames[AcId].flight_plan_path);
- parse_ac_fp(AcId, FlightPlanPath);
-
- //parse airframe file
- char AirframePath[BUFLEN];
- strcpy(AirframePath, PprzFolder);
- strcat(AirframePath, DevNames[AcId].airframe_path);
- parse_ac_af(AcId, AirframePath);
-
- //parse dl_settings for launch & kill throttle
- char SettingsPath[BUFLEN];
- strcpy(SettingsPath, PprzFolder);
- strcat(SettingsPath, DevNames[AcId].settings_path);
- parse_dl_settings(AcId, SettingsPath);
- }
-
- ret = xmlTextReaderRead(reader);
- }
-
- xmlFreeTextReader(reader);
- if (ret != 0) {
- if (verbose) {
- printf("App Server: failed to parse %s\n", xmlFileName);
- fflush(stdout);
- }
- }
+ //Split arg0 to get process id and request id
+ char * mtok;
+ mtok = strtok (argv[0],"_");
+ while (mtok != NULL || i>2)
+ {
+ RmId[i]= atoi(mtok);
+ i++;
+ mtok = strtok (NULL, "_");
}
- else{
+
+ //Check whether process id and request id matches or not
+ if ( RmId[0]== ProcessID && RmId[1] <= RequestID) {
+ int inc_device_id=atoi(argv[1]);
+
+ //Save Device Name
+ strcpy(DevNames[inc_device_id].name, argv[7] );
+
+ //Save color
+ strcpy(DevNames[inc_device_id].color, argv[6]);
+
+ //Save Flight Plan Path
+ //check if file is local
+ if ((strncmp( argv[2], "file://", strlen("file://"))) == 0) {
+ sprintf(DevNames[inc_device_id].flight_plan_path, "%s", argv[2]+7);
+ }
+ else {
+ printf("App Server: App server only works with local files! (Flight Plan Path:%s)\n", argv[2]);
+ return;
+ }
+
+ //Save Airframe Path
+ //check if file is local
+ if ((strncmp( argv[3], "file://", strlen("file://"))) == 0) {
+ sprintf(DevNames[inc_device_id].airframe_path, "%s", argv[3]+7);
+ }
+ else {
+ printf("App Server: App server works only with local files! (Airframe Path:%s)\n", argv[3]);
+ return;
+ }
+
+ //Save Settings Path
+ //check if file is local
+ if ((strncmp( argv[5], "file://", strlen("file://"))) == 0) {
+ sprintf(DevNames[inc_device_id].settings_path, "%s", argv[5]+7);
+ }
+ else {
+ printf("App Server: App server works only with local files! (Settings Path:%s)\n", argv[5]);
+ return;
+ }
+
+ // Init some variables (-1 means no settings in xml file)
+ DevNames[inc_device_id].dl_launch_ind = -1;
+ DevNames[inc_device_id].kill_thr_ind = -1;
+ DevNames[inc_device_id].flight_altitude_ind = -1;
+
+ //Parse airframe files..
+ parse_ac_af(inc_device_id, DevNames[inc_device_id].airframe_path);
+
+ //Parse flight plan
+ parse_ac_fp(inc_device_id, DevNames[inc_device_id].flight_plan_path);
+
+ //Parse settings
+ parse_ac_settings(inc_device_id, DevNames[inc_device_id].settings_path);
+
if (verbose) {
- printf("App Server: Unable to open %s\n", xmlFileName);
+ printf("%s configuration saved. : (Id: %d Color:%s)\n", DevNames[inc_device_id].name,inc_device_id, DevNames[inc_device_id].color);
+ fflush(stdout);
+ printf("\tFlight_P Path:\t %s \n", DevNames[inc_device_id].flight_plan_path);
+ fflush(stdout);
+ printf("\tAirframe Path:\t %s \n", DevNames[inc_device_id].airframe_path);
+ fflush(stdout);
+ printf("\tSettings Path:\t %s \n", DevNames[inc_device_id].settings_path);
fflush(stdout);
}
+ //everything is awesome! AC data is ready to be served.
+
}
- return;
-} // end of XMLParseDoc function
+}
+
+void on_app_server_AIRCRAFTS (IvyClientPtr app, void *user_data, int argc, char *argv[]) {
+
+ int i=0;
+ int RmId[2];
+
+ /*
+ * RmId[0] >> ProcessID of incoming MsgProcessId
+ * RmId[1] >> RequestID of incoming MsgProcessId
+ */
+
+ //Split arg0 to get process id and request id
+ char * mtok;
+ mtok = strtok (argv[0],"_");
+ while (mtok != NULL || i>2)
+ {
+ RmId[i]= atoi(mtok);
+ i++;
+ mtok = strtok (NULL, "_");
+ }
+
+ //Check whether process id and request id matches or not
+ if ( RmId[0]== ProcessID && RmId[1] <= RequestID) {
+ i=0;
+ char * mtok2;
+ mtok2 = strtok (argv[1],",");
+
+ while (mtok2 != NULL || i> MAXDEVICENUMB) {
+ request_ac_config(atoi(mtok2));
+ mtok2 = strtok (NULL, ",");
+ i++;
+ }
+
+ }
+
+}
// Print help message
void print_help() {
@@ -717,24 +804,30 @@ void print_help() {
printf(" -u \tfor sending AC data (default: %d)\n", udp_port);
printf(" -b \tdefault is %s\n", defaultIvyBus);
printf(" -p \tpassword for connection with control capabilities (default is %s)\n", defaultAppPass);
+ printf(" -utcp \t\tUse TCP communication to send ivy messages (default: UDP )\n");
printf(" -v\tverbose\n");
printf(" -h --help show this help\n");
}
+gboolean request_ac_list(gpointer data) {
+ RequestID++;
+ IvySendMsg("app_server %d_%d AIRCRAFTS_REQ" ,ProcessID, RequestID);
+ return FALSE;
+}
+
int main(int argc, char **argv) {
int i;
+ //Get process id
+ ProcessID= getpid();
// default password
+
AppPass = defaultAppPass;
// try environment variable first, set to default if failed
IvyBus = getenv("IVYBUS");
if (IvyBus == NULL) IvyBus = defaultIvyBus;
- // Look for paparazzi folder (PAPARAZZI_HOME or assume local path by default)
- char* PprzFolder = getenv("PAPARAZZI_HOME");
- if (PprzFolder == NULL) PprzFolder = defaultPprzFolder;
-
// Parse options
for (i = 1; i < argc; ++i) {
if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) {
@@ -756,6 +849,9 @@ int main(int argc, char **argv) {
else if (strcmp(argv[i], "-v") == 0) {
verbose = 1;
}
+ else if (strcmp(argv[i], "-utcp") == 0) {
+ uTCP = 1;
+ }
else {
printf("App Server: Unknown option\n");
print_help();
@@ -765,16 +861,17 @@ int main(int argc, char **argv) {
if (verbose) {
printf("### Paparazzi App Server ###\n");
- printf("Using Paparazzi Folder : %s\n", PprzFolder);
printf("Server listen port (TCP) : %d\n", tcp_port);
- printf("Server broadcast port (UDP) : %d\n", udp_port);
+ if (uTCP) {
+ printf("Server using TCP communication..\n");
+ }else{
+ printf("Server broadcast port (UDP) : %d\n", udp_port);
+ }
printf("Control Pass : %s\n", AppPass);
printf("Ivy Bus : %s\n", IvyBus);
fflush(stdout);
}
- //Parse conf.xml
- parse_ac_data(PprzFolder);
//Create tcp listener
#if !GLIB_CHECK_VERSION (2, 35, 1)
@@ -797,10 +894,12 @@ int main(int argc, char **argv) {
g_signal_connect(service, "incoming", G_CALLBACK(new_connection), NULL);
//Here comes the ivy bindings
- IvyInit ("PPRZ_App_Server", "Papparazzi App Server Ready", NULL, NULL, NULL, NULL);
+ IvyInit ("PPRZ_App_Server", "Papparazzi App Server Ready!", NULL, NULL, NULL, NULL);
- IvyBindMsg(Ivy_All_Msgs, NULL, "(^ground .*)");
- IvyBindMsg(Ivy_All_Msgs, NULL, "(^\\S* AIRSPEED (\\S*) (\\S*) (\\S*) (\\S*))");
+ IvyBindMsg(Ivy_All_Msgs, NULL, "(^ground (\\S*) (\\S*) .*)");
+ IvyBindMsg(on_app_server_NEW_AC, NULL, "ground NEW_AIRCRAFT (\\S*)");
+ IvyBindMsg(on_app_server_GET_CONFIG, NULL, "(\\S*) ground CONFIG (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+ IvyBindMsg(on_app_server_AIRCRAFTS, NULL, "(\\S*) ground AIRCRAFTS (\\S*)");
IvyStart(IvyBus);
GMainLoop *loop = g_main_loop_new(NULL, FALSE);
@@ -809,6 +908,9 @@ int main(int argc, char **argv) {
printf("Starting App Server\n");
fflush(stdout);
}
+ IvySendMsg("app_server");
+
+ g_timeout_add(100, request_ac_list, NULL);
g_main_loop_run(loop);
@@ -818,4 +920,3 @@ int main(int argc, char **argv) {
}
return 0;
}
-
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 610094afc3..446f3d9609 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -136,7 +136,11 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.gps_Pacc <- ivalue "Pacc"
| "ESTIMATOR" ->
a.alt <- fvalue "z";
- a.climb <- fvalue "z_dot"
+ a.climb <- fvalue "z_dot";
+ if a.gps_mode = _3D then
+ a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt)
+ else
+ a.agl <- a.alt -. a.ground_alt
| "AIRSPEED" ->
a.airspeed <- fvalue "airspeed"
| "DESIRED" ->
@@ -152,14 +156,22 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.desired_climb <- (try fvalue "climb" with _ -> fvalue "desired_climb");
begin try a.desired_course <- norm_course (fvalue "course") with _ -> () end
| "NAVIGATION_REF" ->
- a.nav_ref <- Some (Utm { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = ivalue "utm_zone" });
- a.ground_alt <- fvalue "ground_alt"
+ a.nav_ref <- Some (Utm { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = ivalue "utm_zone" });
+ a.ground_alt <- fvalue "ground_alt";
+ if a.gps_mode = _3D then
+ a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt)
+ else
+ a.agl <- a.alt -. a.ground_alt
| "NAVIGATION_REF_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
let geo = make_geo (float lat /. 1e7) (float lon /. 1e7) in
a.nav_ref <- Some (Geo geo);
- a.ground_alt <- fvalue "ground_alt"
+ a.ground_alt <- fvalue "ground_alt";
+ if a.gps_mode = _3D then
+ a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt)
+ else
+ a.agl <- a.alt -. a.ground_alt
| "ATTITUDE" ->
let roll = fvalue "phi"
and pitch = fvalue "theta" in
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index af1b52da70..0a34128536 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -511,7 +511,8 @@ let check_alerts = fun a ->
"level", Pprz.String level;
"value", Pprz.Float a.bat] in
Alerts_Pprz.message_send my_id "BAT_LOW" vs in
- if a.bat < catastrophic_level then send "CATASTROPHIC"
+ if a.bat < 1. then send "INVALID"
+ else if a.bat < catastrophic_level then send "CATASTROPHIC"
else if a.bat < critic_level then send "CRITIC"
else if a.bat < warning_level then send "WARNING"
diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml
index 2509004a11..32cbd0052d 100644
--- a/sw/ground_segment/tmtc/server_globals.ml
+++ b/sw/ground_segment/tmtc/server_globals.ml
@@ -4,7 +4,7 @@ let hostname = ref "localhost"
(** FIXME: Should be read from messages.xml *)
let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|]
-let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF"|]
+let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD"|]
let _AUTO2 = 2
let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
diff --git a/sw/include/std.h b/sw/include/std.h
index f75c36c063..00b8f313da 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -107,32 +107,32 @@ typedef uint8_t unit_t;
#define ABS(val) ((val) < 0 ? -(val) : (val))
#endif
-#define Bound(_x, _min, _max) { if (_x > _max) _x = _max; else if (_x < _min) _x = _min; }
+#define Bound(_x, _min, _max) { if (_x > (_max)) _x = (_max); else if (_x < (_min)) _x = (_min); }
#define BoundInverted(_x, _min, _max) { \
- if ((_x < _min) && (_x > _max)) { \
- if (abs(_x - _min) < abs(_x - _max)) \
- _x = _min; \
+ if ((_x < (_min)) && (_x > (_max))) { \
+ if (abs(_x - (_min)) < abs(_x - (_max))) \
+ _x = (_min); \
else \
- _x = _max; \
+ _x = (_max); \
} \
}
#define BoundWrapped(_x, _min, _max) { \
- if (_max > _min) \
+ if ((_max) > (_min)) \
Bound(_x, _min, _max) \
else \
BoundInverted(_x, _min, _max) \
}
#define BoundAbs(_x, _max) Bound(_x, -(_max), (_max))
#define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) )
-#define ChopAbs(x, max) Chop(x, -max, max)
+#define ChopAbs(x, max) Chop(x, -(max), (max))
#define DeadBand(_x, _v) { \
- if (_x > _v) \
- _x = _x -_v; \
- else if (_x < -_v) \
- _x = _x +_v; \
- else \
- _x = 0; \
+ if (_x > (_v)) \
+ _x = _x -(_v); \
+ else if (_x < -(_v)) \
+ _x = _x +(_v); \
+ else \
+ _x = 0; \
}
#define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b))
diff --git a/sw/tools/airframe_editor/airframe_editor.py b/sw/tools/airframe_editor/airframe_editor.py
index 2d7417dc1d..403412da66 100755
--- a/sw/tools/airframe_editor/airframe_editor.py
+++ b/sw/tools/airframe_editor/airframe_editor.py
@@ -44,7 +44,7 @@ class AirframeEditor:
def find_firmwares(self, widget):
list_of_firmwares = paparazzi.get_list_of_firmwares()
self.update_combo(self.firmwares_combo, list_of_firmwares)
-
+
def find_modules(self, widget):
list_of_modules = paparazzi.get_list_of_modules()
self.update_combo(self.modules_combo, list_of_modules)
@@ -117,7 +117,7 @@ class AirframeEditor:
# Constructor Functions
def fill_tree_from_airframe(self):
-
+
# create a TreeStore with one string column to use as the model
self.treestore = gtk.TreeStore(str, object)
@@ -134,9 +134,9 @@ class AirframeEditor:
self.tvcolumn.pack_start(self.cell, True)
self.tvcolumn.add_attribute(self.cell, 'text', 0)
self.treeview.set_reorderable(True)
-
+
def fill_datagrid_from_section(self):
-
+
# create a TreeStore with one string column to use as the model
self.gridstore = gtk.ListStore(str, str, str, str, str)
@@ -233,7 +233,7 @@ class AirframeEditor:
self.btnExit.set_tooltip_text("Close application")
self.btnOpen = gtk.Button("Open")
- self.btnOpen.connect("clicked", self.open)
+ self.btnOpen.connect("clicked", self.open)
self.btnRun = gtk.Button("Reorganize XML")
self.btnRun.connect("clicked", self.reorganize_xml)
@@ -271,7 +271,7 @@ class AirframeEditor:
self.boards_combo = gtk.combo_box_entry_new_text()
self.find_boards(self.boards_combo)
-
+
self.firmwarebar = gtk.HBox()
self.firmwarebar.pack_start(self.btnFirmwares)
@@ -327,7 +327,7 @@ class AirframeEditor:
self.load_airframe_xml()
- ##### Bottom
+ ##### Bottom
self.searchbar = gtk.HBox()
diff --git a/sw/tools/airframe_editor/paparazzi.py b/sw/tools/airframe_editor/paparazzi.py
index aa9dc657a3..772a6d8c4b 100755
--- a/sw/tools/airframe_editor/paparazzi.py
+++ b/sw/tools/airframe_editor/paparazzi.py
@@ -36,7 +36,7 @@ def get_list_of_files(directory, extension):
def get_list_of_modules():
return get_list_of_files( modules_dir, ".xml")
-
+
def get_list_of_firmwares():
return get_list_of_files( firmwares_dir, ".makefile")
diff --git a/sw/tools/flash_scripts/bmp_jtag_flash.scr b/sw/tools/flash_scripts/bmp_jtag_flash.scr
index 4728b18578..284f8acd7a 100644
--- a/sw/tools/flash_scripts/bmp_jtag_flash.scr
+++ b/sw/tools/flash_scripts/bmp_jtag_flash.scr
@@ -1,3 +1,4 @@
monitor jtag_scan
attach 1
load
+kill
diff --git a/sw/tools/flash_scripts/bmp_swd_flash.scr b/sw/tools/flash_scripts/bmp_swd_flash.scr
index 27663c8faa..754392f8c9 100644
--- a/sw/tools/flash_scripts/bmp_swd_flash.scr
+++ b/sw/tools/flash_scripts/bmp_swd_flash.scr
@@ -2,3 +2,4 @@ monitor version
monitor swdp_scan
attach 1
load
+kill
diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml
index a7f52591c3..d733e7251b 100644
--- a/sw/tools/generators/gen_aircraft.ml
+++ b/sw/tools/generators/gen_aircraft.ml
@@ -80,15 +80,13 @@ let dump_module_section = fun xml f ->
fprintf f "# modules makefile section\n";
fprintf f "####################################################\n";
fprintf f "\n# include modules directory for all targets\n";
- (* get dir and target list *)
+ (* get dir list *)
let dir_list = Gen_common.get_modules_dir modules in
- (**
- let target_list = union_of_lists (List.map (fun (m,_) -> get_targets_of_module m) modules) in
- List.iter (fun target -> fprintf f "%s.CFLAGS += -Imodules -Iarch/$(ARCH)/modules\n" target) target_list;
- **)
(** include modules directory for ALL targets and not just the defined ones **)
fprintf f "$(TARGET).CFLAGS += -Imodules -Iarch/$(ARCH)/modules\n";
List.iter (fun dir -> let dir_name = (String.uppercase dir)^"_DIR" in fprintf f "%s = modules/%s\n" dir_name dir) dir_list;
+ (* add vpath for external modules *)
+ List.iter (fun m -> match m.vpath with Some vp -> fprintf f "VPATH += %s\n" vp | _ -> ()) modules;
(* parse each module *)
List.iter (fun m ->
let name = ExtXml.attrib m.xml "name" in
@@ -129,13 +127,21 @@ let dump_module_section = fun xml f ->
| "define" ->
List.iter (fun target ->
let value = try "="^(Xml.attrib field "value") with _ -> ""
- and name = Xml.attrib field "name" in
+ and name = Xml.attrib field "name"
+ and vpath = match m.vpath with Some vp -> vp^"/" | _ -> "" in
let flag_type = match (ExtXml.attrib_or_default field "type" "define") with
"define" | "D" -> "D"
- | "include" | "I" -> "I"
+ | "include" | "I" -> "I"^vpath
+ | "raw" -> ""
| _ -> "D" in
fprintf f "%s.CFLAGS += -%s%s%s\n" target flag_type name value
) targets
+ | "flag" ->
+ List.iter (fun target ->
+ let value = Xml.attrib field "value"
+ and name = Xml.attrib field "name" in
+ fprintf f "%s.%s += -%s\n" target name value
+ ) targets
| "file" ->
let name = Xml.attrib field "name" in
let dir_name = ExtXml.attrib_or_default field "dir" ("$("^dir_name^")") in
diff --git a/sw/tools/generators/gen_autopilot.ml b/sw/tools/generators/gen_autopilot.ml
index 4001d4bdaf..b510ac929b 100644
--- a/sw/tools/generators/gen_autopilot.ml
+++ b/sw/tools/generators/gen_autopilot.ml
@@ -215,12 +215,19 @@ let print_ap_periodic = fun modes ctrl_block main_freq out_h ->
lprintf out_h "%s" cond
with _ -> ()
in
+ (** check control_block *)
+ let get_control_block = fun c ->
+ try
+ List.find (fun n -> (Xml.attrib c "name") = (Xml.attrib n "name")) ctrl_block
+ with
+ Not_found -> failwith (sprintf "Could not find block %s" (Xml.attrib c "name"))
+ in
(** Print a control block *)
let print_ctrl = fun ctrl ->
List.iter (fun c ->
match (Xml.tag c) with
"call" -> print_call c
- | "call_block" -> List.iter print_call (Xml.children (List.find (fun n -> (Xml.attrib c "name") = (Xml.attrib n "name")) ctrl_block))
+ | "call_block" -> List.iter print_call (Xml.children (get_control_block c))
| _ -> ()
) (Xml.children ctrl)
in
@@ -261,7 +268,7 @@ let print_ap_periodic = fun modes ctrl_block main_freq out_h ->
let ctrl_freq = try int_of_string (Xml.attrib c "freq") with _ -> main_freq in
let prescaler = main_freq / ctrl_freq in
match prescaler with
- 0 -> failwith "Autopilot Core Error: control freq higher than main freq"
+ 0 -> failwith (sprintf "Autopilot Core Error: control freq (%d) higher than main_freq (%d)" ctrl_freq main_freq)
| 1 -> print_ctrl c (* no prescaler if running at main_freq *)
| _ -> print_prescaler prescaler c
) (get_control m);
@@ -326,12 +333,9 @@ let () =
failwith (Printf.sprintf "Usage: %s airframe_xml_file out_h_file" Sys.argv.(0));
let xml_file = Sys.argv.(1)
and h_file = Sys.argv.(2) in
- try
- let xml = Xml.parse_file xml_file in
- let (autopilot, ap_freq) = Gen_common.get_autopilot_of_airframe xml in
- gen_autopilot ap_freq autopilot h_file;
- ()
- with
+ let (autopilot, ap_freq) = try
+ Gen_common.get_autopilot_of_airframe (Xml.parse_file xml_file)
+ with
Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
| Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.prove_error e); exit 1
| Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
@@ -341,3 +345,9 @@ let () =
fprintf out_h "/*** Sorry, no autopilot file found ***/\n";
close_out out_h;
exit 0
+ in
+ try
+ gen_autopilot ap_freq autopilot h_file;
+ ()
+ with
+ Not_found -> fprintf stderr "gen_autopilot: What the heck? Something went wrong...\n"; exit 1
diff --git a/sw/tools/generators/gen_common.ml b/sw/tools/generators/gen_common.ml
index 54ede59123..b8fe7b3f88 100644
--- a/sw/tools/generators/gen_common.ml
+++ b/sw/tools/generators/gen_common.ml
@@ -24,7 +24,7 @@
open Printf
-type module_conf = { xml : Xml.xml; file : string; param : Xml.xml list; extra_targets : string list; }
+type module_conf = { xml : Xml.xml; file : string; vpath : string option; param : Xml.xml list; extra_targets : string list; }
let (//) = Filename.concat
@@ -68,7 +68,7 @@ let targets_of_field = fun field default ->
(** [get_autopilot_of_airframe xml]
* Returns (autopilot xml, main freq) from airframe xml file *)
let get_autopilot_of_airframe = fun xml ->
- (* extract all "modules" sections *)
+ (* extract all "autopilot" sections *)
let section = List.filter (fun s -> compare (Xml.tag s) "autopilot" = 0) (Xml.children xml) in
(* Raise error if more than one modules section *)
match section with
@@ -95,9 +95,15 @@ let rec get_modules_of_airframe = fun xml ->
(* if only one section, returns a list of configuration *)
let t_global = targets_of_field modules "" in
let get_module = fun m t ->
- let file = modules_dir // ExtXml.attrib m "name" in
+ (* extract dir name if any and add paparazzi_home path if dir path is not global *)
+ let (dir, vpath) = try
+ let dir = ExtXml.attrib m "dir" in
+ let dir = if Filename.is_relative dir then Env.paparazzi_home // dir else "" in
+ (dir, Some dir)
+ with _ -> (modules_dir, None) in
+ let file = dir // ExtXml.attrib m "name" in
let targets = singletonize (t @ targets_of_field m "") in
- { xml = ExtXml.parse_file file; file = file; param = Xml.children m; extra_targets = targets }
+ { xml = ExtXml.parse_file file; file = file; vpath = vpath; param = Xml.children m; extra_targets = targets }
in
let modules_list = List.map (fun m ->
if compare (Xml.tag m) "load" <> 0 then Xml2h.xml_error "load";
diff --git a/sw/tools/generators/gen_common.mli b/sw/tools/generators/gen_common.mli
index 295c1e67ff..79862aa9b8 100644
--- a/sw/tools/generators/gen_common.mli
+++ b/sw/tools/generators/gen_common.mli
@@ -25,10 +25,11 @@
(* Module configuration:
* Xml node
* file name
+ * optional vpath
* parameters
* extrat targets
*)
-type module_conf = { xml : Xml.xml; file : string; param : Xml.xml list; extra_targets : string list; }
+type module_conf = { xml : Xml.xml; file : string; vpath : string option; param : Xml.xml list; extra_targets : string list; }
(* Modules directory *)
val modules_dir : string
diff --git a/sw/tools/process_exif/process_exif.py b/sw/tools/process_exif/process_exif.py
index 9394245504..31088f5097 100644
--- a/sw/tools/process_exif/process_exif.py
+++ b/sw/tools/process_exif/process_exif.py
@@ -32,10 +32,10 @@ M_PI_4=(M_PI/4)
def RadOfDeg( deg ):
return (deg / M_PI) * 180.
-# converts UTM coords to lat/long. Equations from USGS Bulletin 1532
-# East Longitudes are positive, West longitudes are negative.
+# converts UTM coords to lat/long. Equations from USGS Bulletin 1532
+# East Longitudes are positive, West longitudes are negative.
# North latitudes are positive, South latitudes are negative
-# Lat and Long are in decimal degrees.
+# Lat and Long are in decimal degrees.
# Written by Chuck Gantz- chuck.gantz@globalstar.com
# ( I had some code here to use GDAL and which looked much simpler, but couldn't get that to work )
@@ -46,33 +46,33 @@ def UTMtoLL( northing, easting, utm_zone ):
a = 6378137; # WGS-84
eccSquared = 0.00669438; # WGS-84
e1 = (1-math.sqrt(1-eccSquared))/(1+math.sqrt(1-eccSquared));
-
+
x = easting - 500000.0; # remove 500,000 meter offset for longitude
y = northing;
-
+
is_northern = northing < 0
if ( not is_northern ):
y -= 10000000.0 # remove 10,000,000 meter offset used for southern hemisphere
-
+
LongOrigin = (utm_zone - 1)*6 - 180 + 3; # +3 puts origin in middle of zone
-
+
eccPrimeSquared = (eccSquared)/(1-eccSquared);
-
+
M = y / k0;
mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*math.sin(2*mu) + (21*e1*e1/16-55*e1*e1*e1*e1/32)*math.sin(4*mu) +(151*e1*e1*e1/96)*math.sin(6*mu);
phi1 = RadOfDeg(phi1Rad);
-
+
N1 = a/math.sqrt(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad));
T1 = math.tan(phi1Rad)*math.tan(phi1Rad);
C1 = eccPrimeSquared*math.cos(phi1Rad)*math.cos(phi1Rad);
R1 = a*(1-eccSquared)/math.pow(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad), 1.5);
D = x/(N1*k0);
-
+
Lat = phi1Rad - (N1*math.tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
Lat = RadOfDeg(Lat)
-
+
Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)*D*D*D*D*D/120)/math.cos(phi1Rad)
Long = LongOrigin + RadOfDeg(Long)
@@ -136,29 +136,29 @@ for line in f:
# Check that there as many photos and pick the indicated one.
# (this assumes the photos were taken correctly without a hiccup)
- # It would never be able to check this anyway, since the camera could stall or
+ # It would never be able to check this anyway, since the camera could stall or
# not interpret the pulse? Leading to an incorrect GPS coordinate.
if len( photos ) < photonr:
print "Photo data %d found, but ran out of photos in directory"%(photonr)
- continue
+ continue
- # I've seen log files with -1 as DC_SHOT number due to an int8 I think. This should be
+ # I've seen log files with -1 as DC_SHOT number due to an int8 I think. This should be
# fixed now, but just in case someone runs this on old data.
if (photonr < 0):
print "Negative photonr found."
continue
- # Pick out photo, open it through exiv2,
+ # Pick out photo, open it through exiv2,
photoname = photos[ photonr - 1 ]
photo = GExiv2.Metadata( photoname )
-
+
photo.set_gps_info(lat, lon, alt)
photo.save_file()
print "Photo %s and photonr %d merged. Lat/Lon/Alt: %f, %f, %f"%(photoname, photonr, lat, lon, alt)
except ValueError as e:
- print "Cannot read line: %s"%(line)
+ print "Cannot read line: %s"%(line)
print "Value error(%s)"%(e)
continue
diff --git a/sw/tools/stm32loader/stm32loader.py b/sw/tools/stm32loader/stm32loader.py
index 67e3b6f625..fe64de11f2 100755
--- a/sw/tools/stm32loader/stm32loader.py
+++ b/sw/tools/stm32loader/stm32loader.py
@@ -257,7 +257,7 @@ class CommandInterface:
if usepbar:
widgets = ['Reading: ', Percentage(),', ', ETA(), ' ', Bar()]
pbar = ProgressBar(widgets=widgets,maxval=lng, term_width=79).start()
-
+
while lng > 256:
if usepbar:
pbar.update(pbar.maxval-lng)
@@ -279,7 +279,7 @@ class CommandInterface:
if usepbar:
widgets = ['Writing: ', Percentage(),' ', ETA(), ' ', Bar()]
pbar = ProgressBar(widgets=widgets, maxval=lng, term_width=79).start()
-
+
offs = 0
while lng > 256:
if usepbar:
@@ -324,7 +324,7 @@ def usage():
if __name__ == "__main__":
-
+
# Import Psyco if available
try:
import psyco
diff --git a/sw/tools/tcp_aircraft_server/phoenix/__init__.py b/sw/tools/tcp_aircraft_server/phoenix/__init__.py
new file mode 100644
index 0000000000..1c49530ff6
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/__init__.py
@@ -0,0 +1,163 @@
+#Copyright 2014, Antoine Drouin
+"""
+Phoenix is a Python library for interacting with Paparazzi
+"""
+
+import math
+
+"""
+Unit convertions
+"""
+def rad_of_deg(d): return d/180.*math.pi
+
+def deg_of_rad(r): return r*180./math.pi
+
+def rps_of_rpm(r): return r*2.*math.pi/60.
+
+def rpm_of_rps(r): return r/2./math.pi*60.
+
+def m_of_inch(i): return i*0.0254
+
+
+"""
+Plotting
+"""
+import matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.gridspec as gridspec
+
+my_title_spec = {'color' : 'k', 'fontsize' : 20 }
+
+def save_if(filename):
+ if filename: matplotlib.pyplot.savefig(filename, dpi=80)
+
+def prepare_fig(fig=None, window_title=None, figsize=(20.48, 10.24), margins=None):
+ if fig == None:
+ fig = plt.figure(figsize=figsize)
+# else:
+# plt.figure(fig.number)
+ if margins:
+ left, bottom, right, top, wspace, hspace = margins
+ fig.subplots_adjust(left=left, right=right, bottom=bottom, top=top,
+ hspace=hspace, wspace=wspace)
+ if window_title:
+ fig.canvas.set_window_title(window_title)
+ return fig
+
+def decorate(ax, title=None, xlab=None, ylab=None, legend=None, xlim=None, ylim=None):
+ ax.xaxis.grid(color='k', linestyle='-', linewidth=0.2)
+ ax.yaxis.grid(color='k', linestyle='-', linewidth=0.2)
+ if xlab:
+ ax.xaxis.set_label_text(xlab)
+ if ylab:
+ ax.yaxis.set_label_text(ylab)
+ if title:
+ ax.set_title(title, my_title_spec)
+ if legend <> None:
+ ax.legend(legend, loc='best')
+ if xlim <> None:
+ ax.set_xlim(xlim[0], xlim[1])
+ if ylim <> None:
+ ax.set_ylim(ylim[0], ylim[1])
+
+
+"""
+Messages
+"""
+
+#: dictionary mapping the C type to its length in bytes (e.g char -> 1)
+TYPE_TO_LENGTH_MAP = {
+ "char" : 1,
+ "uint8" : 1,
+ "int8" : 1,
+ "uint16" : 2,
+ "int16" : 2,
+ "uint32" : 4,
+ "int32" : 4,
+ "float" : 4,
+ "double" : 8,
+}
+
+#: dictionary mapping the C type to correct format string
+TYPE_TO_PRINT_MAP = {
+ float : "%f",
+ str : "%s",
+ chr : "%c",
+ int : "%d"
+}
+
+ACID_ALL = 0xFF
+ACID_TEST = 0xFE
+ACID_GROUNDSTATION = 0xFD
+
+#: dictionary mapping debug types to format characters
+DEBUG_MESSAGES = {
+ "DEBUG_UINT8" : "%d",
+ "DEBUG_INT32" : "%d",
+ "DEBUG_FLOAT" : "%#f"
+}
+
+
+
+
+"""
+Binary logs
+
+See format description in sw/airborne/subsystems/datalink/fms_link.c
+
+"""
+
+import struct
+
+def hex_of_bin(b): return ' '.join( [ "%02X" % ord( x ) for x in b ] )
+import pdb
+
+def read_binary_log(filename, tick_freq = 2*512.):
+ f = open(filename, "rb")
+ d = f.read()
+ packet_header_len = 6
+ msg_header_len = 2
+
+ def read_packet(d, packet_start):
+ payload_start = packet_start+packet_header_len
+ timestamp, payload_len = struct.unpack("IH", d[packet_start:payload_start])
+ msgs = read_packet_payload(d, payload_start, payload_len)
+ next_packet = payload_start+payload_len+2
+ return timestamp, msgs, next_packet
+
+ def read_packet_payload(d, s, l):
+ msgs = []
+ packet_end = s+l; msg_start = s
+ while msg_start= t_min and t<= t_max:
+ for id, payload in msgs:
+ m = protocol.get_message_by_id('telemetry', id)
+ try: i = msg_names.index(m.name)
+ except: pass
+ finally: ret[i]['time'].append(t); ret[i]['data'].append(m.unpack_scaled_values(payload))
+ return ret
diff --git a/sw/tools/tcp_aircraft_server/phoenix/messages.py b/sw/tools/tcp_aircraft_server/phoenix/messages.py
new file mode 100644
index 0000000000..30aad17d1f
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/messages.py
@@ -0,0 +1,621 @@
+#Copyright 2014, Antoine Drouin
+# vim: ai ts=4 sts=4 et sw=4
+
+import re
+import os.path
+import struct
+import logging
+import pdb
+
+import phoenix
+from . import xmlobject
+
+LOG = logging.getLogger('PhoenixMessages')
+LOG.setLevel(logging.DEBUG)
+#ch = logging.StreamHandler()
+#ch.setLevel(logging.DEBUG)
+#LOG.addHandler(ch)
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class Field:
+ """
+ **Attributes:**
+ - name : field name
+
+ - ctype : string representing the C type of the field,
+ i.e. uint8[ARRAY_LEN] or uint8[]
+ - type : string representing the C type of a single
+ instance of the field, i.e. uint8
+
+ - is_array :
+
+ - num_elements :
+
+ - length : the number of bytes to store the field,
+ i.e. sizeof(uint8) * ARRAY_LEN
+ - pytype : the python type used to store the field value
+ """
+
+ ARRAY_RE = re.compile("^([a-z0-9]+)\[(\d{,})\]$")
+
+ def __init__(self, f, **kwargs):
+ self.name = f.name
+ self.ctype = f.type
+ m = self.ARRAY_RE.match(f.type)
+ #check if it is an array
+ if m:
+ self.is_array = True
+ elmt_type, elmt_nb = m.groups()
+ self.type = elmt_type
+ self.element_length = phoenix.TYPE_TO_LENGTH_MAP[elmt_type]
+ if elmt_nb != '':
+ self.num_elements = int(elmt_nb)
+ self.length = phoenix.TYPE_TO_LENGTH_MAP[elmt_type] * self.num_elements
+ else:
+ self.num_elements = -1
+ self.length = -1
+ else:
+ self.is_array = False
+ self.type = f.type
+ self.num_elements = 1
+ self.length = phoenix.TYPE_TO_LENGTH_MAP[f.type]
+ self.element_length = None
+
+ #cache the python type for speed
+ if self.type == "float" or self.type == "double":
+ self.pytype = float
+ elif self.type == "char":
+ if self.is_array:
+ self.pytype = str
+ else:
+ self.pytype = chr
+ else:
+ self.pytype = int
+
+
+ def __str__(self):
+ return "" % (self.name, self.ctype)
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class Message:
+ """
+ **Attributes:**
+ - id: message integer id (from messages.xml)
+ - name: message name (upper case) (from messages.xml)
+ - fields: a list of :class:`Field` object (type according to field_class)
+ - size: the length of the message (in bytes)
+ - num_values: the total number of values in the message (as a
+ field can have multiple elements, such as if it is an array)
+ - num_fields: the number of fields in the message
+ - doc: documentation string
+ - is_command: is the message a command (i.e. needs to be ACK'd)
+ """
+ def __init__(self, m, field_klass, **field_kwargs):
+ self._field_kwargs = field_kwargs
+ self.name = m.name.upper()
+ if int(m.id) <= 255 and int(m.id) > 0:
+ self.id = int(m.id)
+ else:
+ raise Exception("Message IDs must be 0 > ID <= 255")
+
+ try:
+ self.fields = [field_klass(f, **self._field_kwargs) for f in xmlobject.ensure_list(m.field)]
+ except AttributeError:
+ self.fields = []
+
+ try:
+ self.is_command = m.command == "1"
+ except AttributeError:
+ self.is_command = False
+
+ try:
+ self.doc = m.doc
+ except AttributeError:
+ self.doc = ""
+
+ self.size = 0
+ self.num_values = 0
+ for f in self.fields:
+ self.size += f.length
+ self.num_values += f.num_elements
+
+ self.num_fields = len(self.fields)
+
+ @property
+ def pretty_name(self):
+ return self.name.replace("_"," ").title()
+
+ def __str__(self):
+ return "" % (self.name, self.id)
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class PyField(Field):
+ """
+ A pythonic object representing a field in a message
+
+ **Attributes:**
+ - is_enum: True if the Field is an enum
+ - struct_format: the :mod:`struct` format string for this type
+ - coef: the unit coefficient for this type (from messages.xml)
+ """
+
+ #: maps user defined names to python struct compatible ids
+ TYPE_TO_STRUCT_MAP = {
+ "char" : "B", #treat char as uint8 internally, only modify how they are displayed
+ "uint8" : "B",
+ "int8" : "b",
+ "uint16": "H",
+ "int16" : "h",
+ "uint32": "I",
+ "int32" : "i",
+ "float" : "f",
+ "double": "f"
+ }
+ #: maps type to range of acceptible values
+ TYPE_TO_RANGE_MAP = {
+ "char" : (0,2**8-1),
+ "uint8" : (0,2**8-1),
+ "int8" : (-2**(8-1),2**(8-1)-1),
+ "uint16": (0,2**16-1),
+ "int16" : (-2**(16-1),2**(16-1)-1),
+ "uint32": (0,2**32-1),
+ "int32" : (-2**(32-1),2**(32-1)-1),
+ "float" : (-3.4e38,3.4e38), #not exactly correct
+ "double": (-3.4e38,3.4e38) #not exactly correct
+ }
+
+ def __init__(self, node, **kwargs):
+ Field.__init__(self, node, **kwargs)
+
+ shared_values = kwargs["shared_values"]
+ self.display_real_unit = False #kwargs["display_real_unit"]
+
+ self.is_enum = False
+ if self.type == "uint8":
+ try:
+ values = node.values
+ if values[0] == "@":
+ self._enum_values = shared_values[values[1:]]
+ else:
+ self._enum_values = values.split("|")
+ self.is_enum = True
+ except AttributeError:
+ self._enum_values = []
+ except KeyError as e:
+ raise Exception("Referenced value does not exist: %s", e)
+
+ if self.is_array:
+ if self.num_elements > 0:
+ self.struct_format = "%d%s" % (self.num_elements, self.TYPE_TO_STRUCT_MAP[self.type])
+ self._size = struct.calcsize(self.struct_format)
+ else:
+ # variable size array, FIXME :(
+ self.struct_format = ""
+ self._size = -1
+ else:
+ self.struct_format = self.TYPE_TO_STRUCT_MAP[self.type]
+ self._size = struct.calcsize(self.struct_format)
+
+ try:
+ self._fstr = node.format
+ except AttributeError:
+ self._fstr = "%s"
+ #if self.is_array:
+ # self._fstr = "%s"
+ #else:
+ # self._fstr = phoenix.TYPE_TO_PRINT_MAP[self.pytype]
+
+ try:
+ self._fstr += " %s" % node.unit
+ except AttributeError:
+ pass
+
+ # if an alternate unit is provided,
+ # update display accordingly
+ try:
+ alt_unit = node.alt_unit
+ except AttributeError:
+ alt_unit = ""
+
+ try:
+ alt_unit_format = node.alt_unit_format
+ except AttributeError:
+ alt_unit_format = "%s"
+
+ try:
+ self.alt_unit_coef = float(node.alt_unit_coef)
+ if self.display_real_unit:
+ self._fstr = alt_unit_format + " " + alt_unit + " ("+ self._fstr +")"
+ else:
+ self._fstr = alt_unit_format + " " + alt_unit
+ except:
+ self.alt_unit_coef = None
+
+ def get_default_value(self):
+ """ Returns a sensible default value for the type """
+ if self.is_array and self.type != "char":
+ return list( [self.pytype() for i in range(self.num_elements)] )
+ else:
+ return self.pytype()
+
+ def interpret_value_from_user_string(self, string, default=None, sep=","):
+ """
+ Tries to interpret the supplied string as best according to the type
+ of the field. For example, the following are legal
+ - "foo bar": if field is char[]
+ - 5: if field is an integer type
+ - 3.4: if field is a float
+ - 1,2,3: if field is an array of integers
+ - ENUM_VALUE: if field is an enum
+
+ In case of failure, the default value (or the value passed in the
+ default argument) is returned
+ """
+ try:
+ if self.is_array and self.type != "char":
+ vals = string.split(sep)
+ if len(vals) != self.num_elements:
+ raise ValueError
+ return list( [self.pytype(v) for v in vals] )
+ elif self.is_enum:
+ try:
+ #first look if the user suppled a string is an enum value
+ return self._enum_values.index(string)
+ except ValueError:
+ #if not, assume it is a number, the constructor of the field
+ #will take care of it
+ return self.pytype(string)
+ else:
+ return self.pytype(string)
+ except ValueError:
+ #invalid user input for type
+ if default:
+ return default
+ else:
+ return self.get_default_value()
+
+ def get_printable_value(self, value):
+ """ Returns a printable string in a human readable format """
+ if self.is_array:
+ if self.type == "char":
+ return "".join([chr(c) for c in value])
+ else:
+ #Returns a printable array, e.g '[1, 2, 3]'
+ return str(value)
+ else:
+ #Return a single formatted number string
+ if self.is_enum:
+ #If this is an uint8 enum type then return the
+ #enum value
+ try:
+ return "%s" % self._enum_values[value]
+ except IndexError:
+ return "?%s?" % value
+ else:
+ if self.alt_unit_coef:
+ if self.display_real_unit:
+ return self._fstr % (self.alt_unit_coef * value, value)
+ else:
+ return self._fstr % (self.alt_unit_coef * value)
+ else:
+ return self._fstr % (value)
+
+ def get_scaled_value(self, value):
+ """
+ Returns the scaled (according to alt_unit_coef). Note, that unlike
+ the other get_ functions, this does not respect the original type of
+ the field. A float is always returned
+ """
+ if self.is_array:
+ if self.type == "char": return value
+ else: return [float(val * self.alt_unit_coef) for val in value]
+ else:
+ if self.is_enum: return value
+ else: return float(value * self.alt_unit_coef) if self.alt_unit_coef else value
+
+ def get_value_range(self):
+ """
+ Returns the legal values this type is allowed to hold. If the type is
+ and enum this returns a n-tuple of all allowed enum values. Otherwise
+ this returns a 2-tuple of the minimum and maximum values this
+ field can hold
+ """
+ if self.is_enum:
+ return self._enum_values
+ else:
+ return self.TYPE_TO_RANGE_MAP[self.ctype]
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class PyMessage(Message):
+ """
+ Represents a message to/from the UAV
+ """
+
+ #Messages are packed in the payload in little endian format
+ MESSAGE_ENDIANESS = "<"
+
+ def __init__(self, name, id, node, **kwargs):
+ Message.__init__(self, node, PyField, **kwargs)
+ self._fields_by_name = {}
+
+ format = self.MESSAGE_ENDIANESS
+ for f in self.fields:
+ format += f.struct_format
+ self._fields_by_name[f.name] = f
+
+ #cache the struct for performace reasons
+ self._struct = struct.Struct(format)
+
+ def __getitem__(self, key):
+ f = self.get_field_by_name(key)
+ if not f:
+ raise KeyError("Could not find field %s" % key)
+ return f
+
+ def get_fields(self):
+ """ Returns a list of :class:`PyField` objects """
+ return self.fields
+
+ def get_field_by_name(self, name):
+ try:
+ return self._fields_by_name[name]
+ except KeyError:
+ return None
+
+ def get_field_index(self, name):
+ i = -1
+ for f in self.fields:
+ i = i + 1
+ if f.name == name:
+ break
+ return i
+
+ def get_field_values(self, vals):
+ """
+ Returns a list of values for each field in the message. The return
+ type is dependent on the type of the field. If the field is an array
+ type then the returned list will contain a tuple of the field array
+ elements. For example ::
+
+
+
+
+
+
+
+
+
+
+
+
+ will return the following ::
+
+ [1, -1, 1000, -1000, 100000, -100000, 1.5, (1, 2, 3)]
+
+ """
+ i = 0
+ v = []
+ for f in self.fields:
+ ne = f.num_elements
+ if f.is_array:
+ v.append( vals[i:i+ne] )
+ else:
+ v.append( vals[i] )
+ i += ne
+ return v
+
+ def pack_values(self, *values):
+ """
+ Assemble the list of field values into a message payload string
+
+ :param values: a list of values of the type expected by the appropriate
+ field. For example, if the 3rd field in the message is a *uint8* then
+ the third value should be an Int
+ """
+ assert len(values) == self.num_values, "%s != %s" % (len(values), self.num_values)
+
+ if self.fields:
+ return self._struct.pack(*values)
+ return ""
+
+ def unpack_values(self, string):
+ """
+ Unlike :func:`get_field_values` this function flattens array
+ fields into the returned list. For example ::
+
+
+
+
+
+
+
+
+
+
+
+
+ will return the following ::
+
+ (1,-1, 1000, -1000, 100000, -100000, 1.5, 1, 2, 3)
+
+ :param string: the message paylod string to unpack
+ """
+ if self.fields:
+ assert type(string) == str
+ assert len(string) == self._struct.size, "%s != %s" % (len(string), self._struct.size)
+ return self._struct.unpack(string)
+ return ()
+
+ def unpack_printable_values(self, string, joiner=" "):
+ """
+ Returns a string, suitable for printing or displaying to the user,
+ of the given message paylod. The string contains values for each
+ field in the message
+
+ :param string: the message payload to unpack
+ :param joiner: the string used between different message fields
+ """
+ if self.fields:
+ vals = self.unpack_values(string)
+ assert len(vals) == self.num_values
+ fvals = self.get_field_values(vals)
+ assert len(fvals) == self.num_fields
+
+ pvals = [self.fields[i].get_printable_value(fvals[i]) for i in range(self.num_fields)]
+
+ if joiner:
+ return joiner.join(pvals)
+ else:
+ return pvals
+ else:
+ return ""
+
+ def unpack_scaled_values(self, string):
+ """
+ As :func:`unpack_values` but returns the values scaled as per the
+ unit coefficient
+ """
+ if self.fields:
+ vals = self.unpack_values(string)
+ assert len(vals) == self.num_values
+ fvals = self.get_field_values(vals)
+ assert len(fvals) == self.num_fields
+
+ return [self.fields[i].get_scaled_value(fvals[i]) for i in range(self.num_fields)]
+ return ()
+
+ def get_default_values(self):
+ """ Returns a list of sensible default values for each field """
+ return [ f.get_default_value() for f in self.fields ]
+
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class MessageClass:
+ """
+ Represents a class of messages in *messages.xml*
+ """
+ def __init__(self, x, shared_values):
+ self.name = x.name
+ self._messages = xmlobject.ensure_list( x.message )
+ self._msgs_by_id = {}
+ self._msgs_by_name = {}
+ for m in self._messages:
+ msg = PyMessage(m.name, m.id, m, shared_values=shared_values)
+ #print m.name
+ self._msgs_by_id[int(m.id)] = msg
+ self._msgs_by_name[m.name] = msg
+
+ def get_message_by_name(self, name):
+ try:
+ return self._msgs_by_name[name]
+ except KeyError:
+ LOG.error("No message with name {:s} in class {:s}".format(name, self.name))
+ return None
+
+ def get_message_by_id(self, id):
+ try:
+ return self._msgs_by_id[id]
+ except KeyError:
+ LOG.error("No message with id {:s} in class {:s}".format(id, self.name))
+ return None
+
+
+
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class Protocol:
+ """
+ A pythonic wrapper for parsing *messages.xml*
+ """
+ def __init__(self, **kwargs):
+ """
+ **Keywords:**
+ - debug - should extra information be printed while parsing
+ *messages.xml*
+ - path - a pathname from which the file can be read
+ - file - an open file object from which the raw xml
+ can be read
+ - raw - the raw xml itself
+ - root - name of root tag, if not reading content
+ """
+ self._debug = kwargs.get("debug", False)
+
+ path = kwargs.get("path")
+ if path and not os.path.exists(path):
+ raise Exception("Could not find message file")
+
+ try:
+ root = xmlobject.XMLFile(**kwargs).root
+ except AttributeError:
+ raise Exception("Invalid XML")
+
+ try:
+ classes = root.msg_class
+ except AttributeError:
+ raise Exception("Missing message class")
+
+ # FIXME: remove that
+ self._values = {}
+ self._msg_classes = {}
+ for msg_class in root.msg_class:
+ if msg_class.name == "telemetry" or msg_class.name == "datalink":
+ self._msg_classes[msg_class.name] = MessageClass(msg_class, self._values)
+
+
+ def __getitem__(self, key):
+ m = self.get_message_by_name(key)
+ if not m:
+ raise KeyError("Could not find message: %s" % key)
+ return m
+
+
+
+ def get_messages(self):
+ """ Returns a list of :class:`PyMessage` objects """
+ return list(self._msgs_by_id.values())
+
+ def get_message_by_name(self, class_name, msg_name):
+ try:
+ return self._msg_classes[class_name].get_message_by_name(msg_name)
+ except KeyError:
+ LOG.error("No message class with name {:s}".format(class_name))
+ return None
+
+ def get_message_by_id(self, class_name, msg_id):
+ try:
+ return self._msg_classes[class_name].get_message_by_id(msg_id)
+ except KeyError:
+ LOG.error("No message class with name {:s}".format(class_name))
+ return None
+
+
diff --git a/sw/tools/tcp_aircraft_server/phoenix/pprz_transport.py b/sw/tools/tcp_aircraft_server/phoenix/pprz_transport.py
new file mode 100644
index 0000000000..7eb58e760b
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/pprz_transport.py
@@ -0,0 +1,210 @@
+#Copyright 2014, Antoine Drouin
+from __future__ import print_function
+import array
+
+STX = 0x99
+#6 non payload bytes; STX, LEN, MSGID, ACID, CK_A, CK_B
+NUM_NON_PAYLOAD_BYTES = 6
+
+class TransportHeaderFooter:
+ """
+ The header/footer of a message
+
+ **Attributes:**
+ - stx: start byte
+ - length: length in bytes (total length, i.e. including header/footer
+ - acid: aircraft ID of sender/destination
+ - msgid: message ID
+ - ck_a: checksum high byte
+ - ck_b: checksum low byte
+ """
+ def __init__(self, stx=STX, length=NUM_NON_PAYLOAD_BYTES, acid=0, msgid=0, ck_a=0, ck_b=0):
+ self.stx = stx
+ self.length = length
+ self.acid = acid
+ self.msgid = msgid
+ self.ck_a = ck_a
+ self.ck_b = ck_b
+
+class Transport:
+ """
+ Class that extracts a wasp payload from a string or sequence of
+ characters (data is sent in little endian byte order)
+
+ Data is expected in the following form ::
+
+ |STX|length|AC_ID|MESSAGE_ID|... payload=(length-6) bytes ...|Checksum A|Checksum B|
+
+ Payload ::
+
+ |... MESSAGE DATA ...|
+
+ There are 6 non payload bytes in a packet (described in :mod:`TransportHeaderFooter`
+ - STX
+ - length
+ - AC_ID
+ - MESSAGE_ID
+ - Checksum A
+ - Checksum B
+ """
+
+ STATE_UNINIT, \
+ STATE_GOT_STX, \
+ STATE_GOT_LENGTH, \
+ STATE_GOT_ACID, \
+ STATE_GOT_MSGID, \
+ STATE_GOT_PAYLOAD, \
+ STATE_GOT_CRC1 = list(range(0,7))
+
+ def __init__(self, check_crc=True, debug=False):
+ self._check_crc = check_crc
+ self._debug = debug
+ self._buf = array.array('c','\0'*256)
+ self._state = self.STATE_UNINIT
+ self._total_len = 0
+ self._payload_len = 0
+ self._payload_idx = 0
+ self._ck_a = 0
+ self._ck_b = 0
+ self._error = 0
+ self._acid = 0
+ self._msgid = 0
+
+ def _debug_msg(self, msg):
+ if self._debug:
+ print(msg)
+
+ def pack_message_with_values(self, header, message, *values):
+ return self.pack_one(
+ header,
+ message,
+ message.pack_values(*values))
+
+ def pack_one(self, header, message, payload):
+ payload_len = len(payload)
+ total_len = payload_len + NUM_NON_PAYLOAD_BYTES
+
+ #create an array big enough to hold data before the payload,
+ #i.e. exclude the checksum
+ buf = array.array('c','\0'*(NUM_NON_PAYLOAD_BYTES - 2))
+
+ buf[0] = chr(header.stx)
+ buf[1] = chr(total_len)
+ buf[2] = chr(header.acid)
+ buf[3] = chr(message.id)
+
+ buf.fromstring(payload)
+
+ ck_a = total_len
+ ck_b = total_len
+ for i in range(2,len(buf)):
+ ck_a = (ck_a + ord(buf[i])) % 256
+ ck_b = (ck_b + ck_a) % 256
+
+ buf.append(chr(ck_a))
+ buf.append(chr(ck_b))
+
+ return buf
+
+ def parse_many(self, string):
+ """
+ Similar to parse_one, but operates on a string, returning
+ multiple payloads if successful
+
+ :returns: A list of payloads strings
+ """
+ payloads = []
+ for c in string:
+ ok,h,p = self.parse_one(c)
+ if ok:
+ payloads.append((h,p))
+ return payloads
+
+ def parse_one(self, c):
+ """
+ Attempts to parse one character. Returns just the payload, and
+ not the data in the transport layer, i.e. it does not return
+ STX, the length, or the checksums
+
+ :returns: The payload string, or an empty string if insuficcient data is available
+ """
+
+ def update_checksum(d):
+ #wrap to 8bit (simulate 8 bit addition)
+ self._ck_a = (self._ck_a + d) % 256
+ self._ck_b = (self._ck_b + self._ck_a) % 256
+
+ def add_to_buf(char, uint8):
+ self._buf[self._payload_idx] = char
+ self._payload_idx += 1
+ update_checksum(uint8)
+
+ payload = ""
+ error = False
+ received = False
+ #convert to 8bit int
+ d = ord(c)
+
+ if self._state == self.STATE_UNINIT:
+ if d == STX:
+ self._state += 1
+ self._ck_a = STX
+ self._ck_b = STX
+ self._debug_msg("-- STX")
+ elif self._state == self.STATE_GOT_STX:
+ self._total_len = d
+ self._payload_len = d - NUM_NON_PAYLOAD_BYTES
+ self._payload_idx = 0
+ update_checksum(d)
+ self._state += 1
+ self._debug_msg("-- SIZE: PL (%s) TOT (%s)" % (self._payload_len, self._total_len))
+ elif self._state == self.STATE_GOT_LENGTH:
+ self._debug_msg("-- ACID: %x" % d)
+ self._acid = d
+ update_checksum(d)
+ self._state += 1
+ elif self._state == self.STATE_GOT_ACID:
+ self._debug_msg("-- MSGID: %x" % d)
+ self._msgid = d
+ update_checksum(d)
+ if self._payload_len == 0:
+ self._state = self.STATE_GOT_PAYLOAD
+ else:
+ self._state += 1
+ elif self._state == self.STATE_GOT_MSGID:
+ add_to_buf(c, d)
+ if self._payload_idx == self._payload_len:
+ self._state += 1
+ self._debug_msg("-- PL")
+ elif self._state == self.STATE_GOT_PAYLOAD:
+ if d != self._ck_a and self._check_crc:
+ error = True
+ self._debug_msg("-- CRC_A ERROR %x v %x" % (d, self._ck_a))
+ else:
+ self._state += 1
+ self._debug_msg("-- CRC_A OK")
+ elif self._state == self.STATE_GOT_CRC1:
+ if d != self._ck_b and self._check_crc:
+ error = True
+ self._debug_msg("-- CRC_B ERROR")
+ else:
+ payload = self._buf[:self._payload_len].tostring()
+ received = True
+ self._state = self.STATE_UNINIT
+ self._debug_msg("-- CRC_B OK")
+
+ if error:
+ self._error += 1
+ self._state = self.STATE_UNINIT
+ elif received:
+ header = TransportHeaderFooter(
+ length=self._total_len,
+ acid=self._acid,
+ msgid=self._msgid,
+ ck_a=self._ck_a,
+ ck_b=self._ck_b)
+ return True, header, payload
+
+ return False, None, None
+
+
diff --git a/sw/tools/tcp_aircraft_server/phoenix/serial_link.py b/sw/tools/tcp_aircraft_server/phoenix/serial_link.py
new file mode 100644
index 0000000000..588959820b
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/serial_link.py
@@ -0,0 +1,107 @@
+#Copyright 2014, Antoine Drouin
+import os, serial, logging
+from gi.repository import GLib, Gio, GObject
+
+import phoenix.pprz_transport as transp
+
+LOG = logging.getLogger('serial_link')
+LOG.setLevel(logging.ERROR)
+#LOG.setLevel(logging.DEBUG)
+import pdb
+
+class PhoenixCommunication(GObject.GObject):
+
+ DEFAULT_PORT = "/dev/ttyO4"
+ DEFAULT_SPEED = 57600
+
+ __gsignals__ = {
+ "message-received" : (GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, [
+ GObject.TYPE_PYOBJECT, #header
+ GObject.TYPE_PYOBJECT]), #payload
+ "status-changed" : (GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, [])
+ }
+
+ def __init__(self, pprz_protocol):
+ GObject.GObject.__init__(self)
+ self._port = PhoenixCommunication.DEFAULT_PORT
+ self._speed = PhoenixCommunication.DEFAULT_SPEED
+ self._watch = None
+ self._is_opened = False;
+ self._transp = transp.Transport(False, False)
+ self._pprz_protocol = pprz_protocol
+ self._serial = None
+ self._available_ports = []
+ self.update_available_ports()
+ self._nb_rx_msgs = 0
+ self._nb_rx_bytes = 0
+ self._nb_tx_msgs = 0
+ self._nb_tx_bytes = 0
+
+
+ def open(self):
+ """
+ Open serial port
+ """
+ LOG.debug("Opening Port: %s @ %d" % (self._port, self._speed))
+ try:
+ self._serial = serial.Serial(self._port, self._speed, timeout=0)
+ self._is_opened = True;
+ except serial.SerialException:
+ self._is_opened = False;
+ self.on_connection_changed()
+
+ def send_msg(self, msg_class, msg_name, *fields_values):
+ """
+ send a message
+ """
+ m = self._pprz_protocol.get_message_by_name(msg_class, msg_name)
+ h = transp.TransportHeaderFooter(acid=0)
+ bin_msg = self._transp.pack_message_with_values(h, m, *fields_values)
+ self._serial.write(bin_msg)
+ self._nb_tx_msgs += 1
+ self._nb_tx_bytes += len(bin_msg)
+
+ def on_connection_changed(self):
+ if self._watch:
+ GObject.source_remove(self._watch)
+ if self._is_opened:
+ self.watch = GObject.io_add_watch(
+ self._serial.fileno(),
+ GObject.IO_IN | GObject.IO_PRI,
+ self.on_serial_data_available,
+ priority=GObject.PRIORITY_HIGH)
+ self.update_available_ports()
+ self.emit("status-changed")
+
+ def get_status(self):
+ return self._is_opened, self._port
+
+ def get_stats(self):
+ return self._nb_rx_msgs, self._nb_rx_bytes, self._nb_tx_msgs, self._nb_tx_bytes
+
+
+ def on_serial_data_available(self, fd, condition):
+ try:
+ data = self._serial.read(4096)
+ LOG.debug("read serial data : %d" % (len(data)))
+ self._nb_rx_bytes += len(data)
+ # print data.encode("hex")
+ msgs = self._transp.parse_many(data)
+ LOG.debug("parsed msg : %d" % (len(msgs)))
+ # print msgs
+ for header, payload in msgs:
+ self.emit("message-received", header, payload)
+ self._nb_rx_msgs += 1
+ return True
+ except serial.SerialException:
+ self._is_opened = False;
+ self.on_connection_changed()
+ return False
+
+ def update_available_ports(self):
+# self.available_ports = filter(lambda x: x.startswith("ttyUSB") or x.startswith("ttyS") , os.listdir("/dev"))
+ self.available_ports = [x for x in os.listdir("/dev") if x.startswith("ttyUSB")]
+ self.available_ports.sort()
+
+ def get_available_ports(self):
+ return self.available_ports
diff --git a/sw/tools/tcp_aircraft_server/phoenix/xmlobject.py b/sw/tools/tcp_aircraft_server/phoenix/xmlobject.py
new file mode 100644
index 0000000000..a56489d520
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/xmlobject.py
@@ -0,0 +1,566 @@
+#Copyright 2014, Antoine Drouin
+"""
+Allows XML files to be operated on like Python objects.
+
+Features:
+ - load XML source from file pathnames, readable file objects or raw strings
+ - add, get and set tag attributes like with python attributes
+ - iterate over nodes
+ - save the modified XMLFile or XMLObject to file
+
+Example XML file::
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+Example usage::
+
+ >>> from xmlobject import XMLFile
+
+ >>> x = XMLFile(path="sample.xml")
+
+ >>> print x
+
+
+ >>> print x.root
+
+
+ >>> print x.root._children
+ [, , ,
+ , ]
+
+ >>> print x.root.person
+ [, ]
+
+ >>> print x.root.person[0].name
+ John Smith
+
+ >>> john = x.root.person[0]
+
+ >>> john.height = 184
+
+ >>> c = john._addNode("crime")
+
+ >>> c.name = "Grand Theft Auto"
+
+ >>> c.date = "4 May, 2005"
+
+ >>> print x.toxml()
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ >>>
+
+"""
+
+import sys, os
+import xml.dom
+import xml.dom.minidom
+from xml.dom.minidom import parse, parseString, getDOMImplementation
+
+impl = getDOMImplementation()
+
+def ensure_list(obj):
+ """
+ ensures the object passed is a list, so it is iterable.
+
+ useful workaround until i decide if XMLNode.foo should always
+ return a list of foo, even if there is only one foo child
+ """
+ if len(obj):
+ return obj
+ else:
+ return [obj]
+
+class MissingRootTag(Exception):
+ """root tag name was not given"""
+
+class InvalidXML(Exception):
+ """failed to parse XML input"""
+
+class CannotSave(Exception):
+ """unable to save"""
+
+class InvalidNode(Exception):
+ """not a valid minidom node"""
+
+class XMLFile:
+ """
+ Allows an xml file to be viewed and operated on
+ as a python object.
+
+ (If you're viewing the epydoc-generated HTML documentation, click the 'show private'
+ link at the top right of this page to see all the methods)
+
+ Holds the root node in the .root attribute, also in an attribute
+ with the same name as this root node.
+ """
+ def __init__(self, **kw):
+ """
+ Create an XMLFile
+
+ Keywords:
+ - path - a pathname from which the file can be read
+ - file - an open file object from which the raw xml
+ can be read
+ - raw - the raw xml itself
+ - root - name of root tag, if not reading content
+
+ Usage scenarios:
+ 1. Working with existing content - you must supply input in
+ one of the following ways:
+ - 'path' must be an existing file, or
+ - 'file' must be a readable file object, or
+ - 'raw' must contain raw xml as a string
+ 2. Creating whole new content - you must give the name
+ of the root tag in the 'root' keyword
+
+ Notes:
+ - Keyword precedence governing existing content is:
+ 1. path (if existing file)
+ 2. file
+ 3. raw
+ - If working with existing content:
+ - if the 'root' is given, then the content's toplevel tag
+ MUST match the value given for 'root'
+ - trying to _save will raise an exception unless 'path'
+ has been given
+ - if not working with existing content:
+ - 'root' must be given
+ - _save() will raise an exception unless 'path' has been given
+ """
+ path = kw.get("path", None)
+ fobj = kw.get("file", None)
+ raw = kw.get("raw", None)
+ root = kw.get("root", None)
+
+ if path:
+ self.path = path
+ try:
+ fobj = file(path)
+ except IOError:
+ pass
+ else:
+ self.path = None
+
+ if fobj:
+ raw = fobj.read()
+
+ if raw:
+ self.dom = xml.dom.minidom.parseString(raw)
+ else:
+ # could not source content, so create a blank slate
+ if not root:
+ # in which case, must give a root node name
+ raise MissingRootTag(
+ "No existing content, so must specify root")
+
+ # ok, create a blank dom
+ self.dom = impl.createDocument(None, root, None)
+
+ # get the root node, save it as attributes 'root' and name of node
+ rootnode = self.dom.documentElement
+
+ # now validate root tag
+ if root:
+ if rootnode.nodeName != root:
+ raise IncorrectRootTag("Gave root='%s', input has root='%s'" % (
+ root, rootnode.nodeName))
+
+ # need this for recursion in XMLNode
+ self._childrenByName = {}
+ self._children = []
+
+ # add all the child nodes
+ for child in self.dom.childNodes:
+ childnode = XMLNode(self, child)
+ #print "compare %s to %s" % (rootnode, child)
+ if child == rootnode:
+ #print "found root"
+ self.root = childnode
+ setattr(self, rootnode.nodeName, self.root)
+
+ def save(self, where=None, obj=None):
+ """
+ Saves the document.
+
+ If argument 'where' is given, saves to it, otherwise
+ tries to save to the original given 'path' (or barfs)
+
+ Value can be a string (taken to be a file path), or an open
+ file object.
+ """
+ obj = obj or self.dom
+
+ if not where:
+ if self._root.path:
+ where = self._root.path
+
+ if isinstance(where, str):
+ where = file(where, "w")
+
+ if not where:
+ raise CannotSave("No save destination, and no original path")
+
+ where.write(obj.toxml())
+ where.flush()
+
+ def saveAs(self, path):
+ """
+ save this time, and all subsequent times, to filename 'path'
+ """
+ self.path = path
+ self.save()
+
+ def toxml(self):
+ return self.dom.toxml()
+
+ def __len__(self):
+ """
+ returns number of child nodes
+ """
+ return len(self._children)
+
+ def __getitem__(self, idx):
+ if isinstance(idx, int):
+ return self._children[idx]
+ else:
+ return self._childrenByName[idx]
+
+
+class XMLNode:
+ """
+ This is the workhorse for the xml object interface
+
+ (If you're viewing the epydoc-generated HTML documentation, click the 'show private'
+ link at the top right of this page to see all the methods)
+
+ """
+
+ # http://docs.python.org/reference/lexical_analysis.html#id6
+ __RESERVED_WORDS = (
+ "and","del","class","from","not","while"
+ "as","elif","global","or","with","assert","else","if",
+ "pass","yield","break","except","import","print",
+ "class","exec","in","raise","continue","finally",
+ "is","return","def","for","lambda","try"
+ )
+
+ def __init__(self, parent, node):
+ """
+ You shouldn't need to instantiate this directly
+ """
+ self._parent = parent
+ if isinstance(parent, XMLFile):
+ self._root = parent
+ else:
+ self._root = parent._root
+ self._node = node
+ self._childrenByName = {}
+ self._children = []
+
+ # add ourself to parent's children registry
+ parent._children.append(self)
+
+ # the deal with named subtags is that we store the first instance
+ # as itself, and with second and subsequent instances, we make a list
+ parentDict = self._parent._childrenByName
+
+ # If the name of the node is a python reserved word then captilize it
+ nodeName = node.nodeName
+ if nodeName in self.__RESERVED_WORDS:
+ nodeName = nodeName.upper()
+
+ if nodeName not in parentDict:
+ parentDict[nodeName] = parent.__dict__[nodeName] = self
+ else:
+ if isinstance(parentDict[nodeName], XMLNode):
+ # this is the second child node of a given tag name, so convert
+ # the instance to a list
+ parentDict[nodeName] = parent.__dict__[nodeName] = [parentDict[nodeName]]
+ parentDict[nodeName].append(self)
+
+ # figure out our type
+ self._value = None
+ if isinstance(node, xml.dom.minidom.Text):
+ self._type = "text"
+ self._value = node.nodeValue
+ elif isinstance(node, xml.dom.minidom.Element):
+ self._type = "node"
+ elif isinstance(node, xml.dom.minidom.Comment):
+ self._type = "comment"
+ self._value = node.nodeValue
+ elif isinstance(node, xml.dom.minidom.DocumentType):
+ #
+ #Ignore doctype, could possibly check it....
+ pass
+ else:
+ raise InvalidNode("node class %s" % node.__class__)
+
+ # and wrap all the child nodes
+ for child in node.childNodes:
+ XMLNode(self, child)
+
+ def _render(self):
+ """
+ Produces well-formed XML of this node's contents,
+ indented as required
+ """
+ return self._node.toxml()
+
+ def __repr__(self):
+ if self._type == "node":
+ return "" % self._node.nodeName
+ else:
+ return "" % self._type
+
+ def __getattr__(self, attr):
+ """
+ Fetches an attribute or child node of this tag
+
+ If it's an attribute, then returns the attribute value as a string.
+
+ If a child node, then:
+ - if there is only one child node of that name, return it
+ - if there is more than one child node of that name, return a list
+ of child nodes of that tag name
+
+ Supports some magic attributes:
+ - _text - the value of the first child node of type text
+ """
+ #print "%s: __getattr__: attr=%s" % (self, attr)
+
+ if attr == '_text':
+ # magic attribute to return text
+ tnode = self['#text']
+ if isinstance(tnode, list):
+ tnode = tnode[0]
+ return tnode._value
+
+ if self._type in ['text', 'comment']:
+ if attr == '_value':
+ return self._node.nodeValue
+ else:
+ raise AttributeError(attr)
+
+ if self._node.hasAttribute(attr):
+ return self._node.getAttribute(attr)
+ elif attr in self._childrenByName:
+ return self._childrenByName[attr]
+
+ #elif attr == 'value':
+ # magic attribute
+
+ else:
+ raise AttributeError(attr)
+
+
+ def __setattr__(self, attr, val):
+ """
+ Change the value of an attribute of this tag
+
+ The magic attribute '_text' can be used to set the first child
+ text node's value
+
+ For example::
+
+ Consider:
+
+
+ foo
+
+
+ >>> somenode
+
+ >>> somenode.child
+
+ >>> somenode.child._text
+ 'foo'
+ >>> somenode._toxml()
+ u'foo'
+ >>> somenode.child._text = 'bar'
+ >>> somenode.child._text
+ 'bar'
+ >>> somenode.child._toxml()
+ u'bar/child>'
+
+ """
+ if attr.startswith("_"):
+
+ # magic attribute for setting _text
+ if attr == '_text':
+ tnode = self['#text']
+ if isinstance(tnode, list):
+ tnode = tnode[0]
+ tnode._node.nodeValue = val
+ tnode._value = val
+ return
+
+ self.__dict__[attr] = val
+ elif self._type in ['text', 'comment']:
+ self._node.nodeValue = val
+ else:
+ # discern between attribute and child node
+ if attr in self._childrenByName:
+ raise Exception("Attribute Exists")
+ self._node.setAttribute(attr, str(val))
+
+ def _keys(self):
+ """
+ Return a list of attribute names
+ """
+ return list(self._node.attributes.keys())
+
+ def _values(self):
+ """
+ Returns a list of (attrname, attrval) tuples for this tag
+ """
+ return [self._node.getAttribute(k) for k in list(self._node.attributes.keys())]
+
+ def _items(self):
+ """
+ returns a list of attribute values for this tag
+ """
+ return [(k, self._node.getAttribute(k)) for k in list(self._node.attributes.keys())]
+
+ def _has_key(self, k):
+ """
+ returns True if this tag has an attribute of the given name
+ """
+ return self._node.hasAttribute(k) or k in self._childrenByName
+
+ def _get(self, k, default=None):
+ """
+ returns the value of attribute k, or default if no such attribute
+ """
+ if self._has_key(k):
+ return getattr(self, k)
+ else:
+ return default
+ def __len__(self):
+ """
+ returns number of child nodes
+ """
+ return len(self._children)
+
+ def __getitem__(self, idx):
+ """
+ if given key is numeric, return the nth child, otherwise
+ try to return the child tag (or list of child tags) having
+ the key as the tag name
+ """
+ #print "__getitem__: idx=%s" % str(idx)
+
+ if isinstance(idx, slice) or isinstance(idx, int):
+ return self._children[idx]
+ elif isinstance(idx, str):
+ return self._childrenByName[idx]
+ else:
+ raise IndexError(idx)
+
+ def _addNode(self, child):
+ """
+ Tries to append a child node to the tree, and returns it
+
+ Value of 'child' must be one of:
+ - a string (in which case it is taken to be the name
+ of the new node's tag)
+ - a dom object, in which case it will be wrapped and added
+ - an XMLNode object, in which case it will be added without
+ wrapping
+ """
+
+ if isinstance(child, XMLNode):
+
+ # add it to our children registry
+ self._children.append(child)
+
+ parentDict = self._childrenByName
+ nodeName = child._node.nodeName
+
+ if nodeName not in parentDict:
+ parentDict[nodeName] = parent.__dict__[nodeName] = child
+ else:
+ if isinstance(parentDict[nodeName], XMLNode):
+ # this is the second child node of a given tag name, so convert
+ # the instance to a list
+ parentDict[nodeName] = self.__dict__[nodeName] = [parentDict[nodeName]]
+ parentDict[nodeName].append(child)
+
+ # and stick it in the dom
+ self._node.appendChild(child._node)
+
+ return child
+
+ elif isinstance(child, str):
+ childNode = self._root.dom.createElement(child)
+ self._node.appendChild(childNode)
+
+ elif isinstance(child, xml.dom.minidom.Element):
+ childNode = child
+ child = childNode.nodeName
+ self._node.appendChild(childNode)
+
+
+ return XMLNode(self, childNode)
+
+ def _addText(self, value):
+ """
+ Tries to append a child text node, with the given text, to the tree,
+ and returns the created node object
+ """
+ childNode = self._root.dom.createTextNode(value)
+ self._node.appendChild(childNode)
+ return XMLNode(self, childNode)
+
+ def _addComment(self, comment):
+ """
+ Tries to append a child comment node (with the given text value)
+ to the tree, and returns the create node object
+ """
+ childNode = self._root.dom.createCommentNode(comment)
+ self._node.appendChild(childNode)
+ return XMLNode(self, childNode)
+
+ def _save(self, where=None):
+ """
+ Generates well-formed XML from just this node, and saves it
+ to a file.
+
+ Argument 'where' is either an open file object, or a pathname
+
+ If 'where' is not given, then saves the entire document tree.
+ """
+ if not where:
+ self._root.save()
+ else:
+ self._root.save(where, self._node)
+
+ def _toxml(self):
+ """
+ renders just this node out to raw xml code
+ """
+ return self._node.toxml()
+
diff --git a/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py b/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py
new file mode 100755
index 0000000000..b454eb0262
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+#-*- coding: utf-8 -*-
+
+#Copyright 2014, Antoine Drouin
+
+from __future__ import print_function
+
+import logging, base64, socket
+from gi.repository import GLib, GObject
+import ivy.ivy as ivy
+ivylogger = logging.getLogger('Ivy')
+ivylogger.setLevel(logging.CRITICAL)
+
+import phoenix.messages
+import phoenix.pprz_transport
+
+from os import path, getenv
+
+# if PAPARAZZI_HOME not set, then assume the tree containing this
+# file is a reasonable substitute
+home_dir = getenv("PAPARAZZI_HOME", path.normpath(path.join(
+ path.dirname(path.abspath(__file__)), '../../../')))
+
+default_ivybus = '127.255.255.255:2010'
+
+class Server(ivy.IvyServer):
+ def __init__(self, bus, tcp_port=4242):
+ ivy.IvyServer.__init__(self, 'TCP_aircraft_server', usesDaemons=True)
+
+ self.nb_msgs = 0
+ self.nb_bytes = 0
+
+ cs = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ cs.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ cs.bind(('', tcp_port))
+ cs.listen(1)
+ GObject.io_add_watch(cs, GObject.IO_IN, self.handle_conn)
+ print("server listening on {:d}".format(tcp_port))
+
+ self.transp = phoenix.pprz_transport.Transport(check_crc=False, debug=False)
+ self.protocol = phoenix.messages.Protocol(path=path.join(home_dir, "conf/messages_ng.xml"), debug=True)
+ self.start(bus)
+
+ GObject.timeout_add(500, self.periodic, priority=GObject.PRIORITY_HIGH)
+
+ def handle_conn(self, sock, cond):
+ conn, addr = sock.accept()
+ print("Connection from {}".format(addr))
+ GObject.io_add_watch(conn, GObject.IO_IN, self.handle_data)
+ return True
+
+ def handle_data(self, sock, cond):
+ buf = sock.recv(4096)
+ if not len(buf):
+ print("Connection closed.")
+ return False
+ else:
+ #print phoenix.hex_of_bin(buf)
+ msgs = self.transp.parse_many(buf)
+ for hdr, payload in msgs:
+ msg = self.protocol.get_message_by_id("telemetry", hdr.msgid)
+ try:
+ ivy_str = '{} {} {}'.format(hdr.acid, msg.name, ' '.join([str(v) for v in msg.unpack_values(payload)]))
+ #print ivy_str
+ self.send_msg(ivy_str)
+ self.nb_msgs += 1
+ self.nb_bytes += len(payload)
+ except:
+ print('FAILED', msg.name)
+ return True
+
+ def periodic(self):
+ print('msgs {} ({} bytes)'.format(self.nb_msgs, self.nb_bytes))
+ return True
+
+if __name__ == '__main__':
+ logging.basicConfig(format='%(levelname)s:%(message)s', level=logging.INFO)
+ server = Server(default_ivybus)
+ GLib.MainLoop().run()