diff --git a/.gitignore b/.gitignore
index 0d10b010ddb..8864c07043a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -78,6 +78,10 @@
/sw/ground_segment/tmtc/server
/sw/ground_segment/tmtc/diadec
+# /sw/ground_segment/joystick
+/sw/ground_segment/joystick/input2ivy
+/sw/ground_segment/joystick/test_stick
+
# /sw/lib/ocaml/
/sw/lib/ocaml/gtk_papget_editor.ml
/sw/lib/ocaml/gtk_papget_text_editor.ml
@@ -109,8 +113,9 @@
/sw/tools/fp_parser.ml
/sw/tools/wiki_gen/wiki_gen
-# /sw/ground_segment/joystick
-/sw/ground_segment/joystick/test_stick
+# /sw/ground_segment/misc
+/sw/ground_segment/misc/davis2ivy
+
# /sw/airborne/arch/lpc21/test/bootloader
/sw/airborne/arch/lpc21/test/bootloader/bl.dmp
diff --git a/Makefile b/Makefile
index 52bbc5537ab..5e928b0fa79 100644
--- a/Makefile
+++ b/Makefile
@@ -43,6 +43,7 @@ AIRBORNE=sw/airborne
COCKPIT=sw/ground_segment/cockpit
TMTC=sw/ground_segment/tmtc
MULTIMON=sw/ground_segment/multimon
+MISC=sw/ground_segment/misc
LOGALIZER=sw/logalizer
SIMULATOR=sw/simulator
MAKE=make PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME)
@@ -69,7 +70,7 @@ OCAMLRUN=$(shell which ocamlrun)
all: commands static conf
-static : lib center tools cockpit multimon tmtc logalizer lpc21iap sim_static static_h usb_lib
+static : lib center tools cockpit multimon tmtc misc logalizer lpc21iap sim_static static_h usb_lib
conf: conf/conf.xml conf/control_panel.xml
@@ -98,6 +99,9 @@ cockpit: lib
tmtc: lib cockpit
cd $(TMTC); $(MAKE) all
+misc:
+ cd $(MISC); $(MAKE) all
+
multimon:
cd $(MULTIMON); $(MAKE)
diff --git a/conf/airframes/CDW/ChimuLisaFw.xml b/conf/airframes/CDW/ChimuLisaFw.xml
index 8a6337cb4f6..0e2fef40aaa 100644
--- a/conf/airframes/CDW/ChimuLisaFw.xml
+++ b/conf/airframes/CDW/ChimuLisaFw.xml
@@ -1,68 +1,17 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
-
-
+
+
+
@@ -78,95 +27,186 @@
-
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
+
-
-
+
+
+
-
-
-
-
+
+
-
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml
index a1fc625fec3..71b912ba1fa 100644
--- a/conf/airframes/CDW/ChimuTinyFwSpi.xml
+++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml
@@ -7,11 +7,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -168,6 +168,7 @@
+
diff --git a/conf/airframes/LAAS/mmlaas_N1.xml b/conf/airframes/LAAS/mmlaas_N1.xml
index 8c08511f7b8..59cacc5ea6a 100644
--- a/conf/airframes/LAAS/mmlaas_N1.xml
+++ b/conf/airframes/LAAS/mmlaas_N1.xml
@@ -24,8 +24,8 @@
-
-
+
+
@@ -40,9 +40,14 @@
-
+
-
+
+
+
+
+
+
@@ -95,7 +100,7 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
+
+FLASH_MODE=IAP
+
+ap.CFLAGS += -DFBW -DAP -DCONFIG=\"tiny_0_99.h\" -DLED -DTIME_LED=1
+ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
+
+ap.srcs += commands.c
+
+ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
+ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
+
+ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
+ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
+
+ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
+ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
+
+ap.CFLAGS += -DINTER_MCU
+ap.srcs += inter_mcu.c
+
+ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2
+ap.srcs += $(SRC_ARCH)/adc_hw.c
+
+ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
+
+ap.srcs += gps_ubx.c gps.c latlong.c
+
+ap.CFLAGS += -DINFRARED -DALT_KALMAN
+ap.srcs += infrared.c estimator.c
+
+ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
+
+# Hack to use the same tuning file than slayer1
+#�ap.CFLAGS += -DUSE_GPIO
+# ap.srcs += $(SRC_ARCH)/gpio.c
+
+# formation
+ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
+ap.srcs += traffic_info.c formation.c snav.c
+
+# external module
+ap.srcs += external.c
+
+# tcas
+ap.CFLAGS += -DTCAS
+ap.srcs += tcas.c
+
+# Harware In The Loop
+
+#ap.CFLAGS += -DHITL
+
+# Losange Cartography with Onboard Camera
+ap.CFLAGS += -DUSE_ONBOARD_CAMERA
+ap.srcs +=nav_survey_losange_carto.c
+
+
+#################################################################################################
+# Config for SITL simulation
+include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DFORMATION -DTRAFFIC_INFO -DTCAS
+sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
+
+# Losange Cartography with Onboard Camera
+sim.CFLAGS += -DUSE_ONBOARD_CAMERA
+sim.srcs += nav_survey_losange_carto.c
+
+#pour les printf de debug
+sim.CFLAGS += -DDEBUG_PRINTF
+
+
+
+
diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
new file mode 100644
index 00000000000..e2428d84fef
--- /dev/null
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -0,0 +1,270 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index 61428ed284e..213a7d95108 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -234,8 +234,8 @@
-
-
+
+
diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 34d651058fe..baba44fc6f7 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -5,7 +5,7 @@
-
+
@@ -28,7 +28,7 @@
-
+
@@ -42,7 +42,7 @@
-
+
@@ -81,9 +81,9 @@
-
-
-
+
+
+
@@ -92,7 +92,7 @@
-
+
@@ -161,7 +161,7 @@
-
+
diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
new file mode 100644
index 00000000000..226be3e4272
--- /dev/null
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -0,0 +1,240 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index 7d1b5657998..f27b129aeb4 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -1,6 +1,6 @@
-
@@ -51,7 +51,7 @@
+
-
+
@@ -215,11 +217,11 @@
-
+
-
+
-
+
diff --git a/conf/airframes/esden/calib/aspirin_012.xml b/conf/airframes/esden/calib/aspirin_012.xml
new file mode 100644
index 00000000000..fe21765684d
--- /dev/null
+++ b/conf/airframes/esden/calib/aspirin_012.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml
new file mode 100644
index 00000000000..d47df390606
--- /dev/null
+++ b/conf/airframes/esden/jt_lisam.xml
@@ -0,0 +1,230 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
index 3f78a121e12..9d87c99f6ab 100644
--- a/conf/airframes/esden/lisa_m_pwm.xml
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -38,8 +38,8 @@
-
-
+
+
@@ -122,12 +122,12 @@
-
+
-
+
@@ -138,22 +138,22 @@
-
-
+
+
-
-
+
+
-
-
+
+
-
-
-
+
+
+
@@ -177,6 +177,12 @@
+
@@ -219,7 +225,7 @@
-
+
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index 01bd57bcdf7..34491a043a3 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -38,7 +38,7 @@
-
+
@@ -49,42 +49,16 @@
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
@@ -138,25 +112,32 @@
-
-
-
+
+
+
-
-
-
+
+
+
-
-
+
+
-
-
-
+
+
+
+
+
@@ -178,7 +159,6 @@
-
@@ -225,6 +205,7 @@
+
diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml
new file mode 100644
index 00000000000..5146d71f5e2
--- /dev/null
+++ b/conf/airframes/esden/synerani_4B.xml
@@ -0,0 +1,249 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile
index 139a7ca919b..3b0758e8bf7 100644
--- a/conf/autopilot/fixedwing.makefile
+++ b/conf/autopilot/fixedwing.makefile
@@ -15,6 +15,7 @@ SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/
SRC_FIRMWARE=firmwares/fixedwing
SRC_SUBSYSTEMS=subsystems
+SRC_MODULES=modules
FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING)
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
index 91b1eb5ecd6..9aa26c0cdf3 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
@@ -10,6 +10,7 @@ ap.CFLAGS += -DUSE_AHRS
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+ap.srcs += math/pprz_trig_int.c
ifdef AHRS_ALIGNER_LED
ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 2faf78d71e0..a1994bcd334 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -218,6 +218,9 @@ jsbsim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -L/usr/lib
jsbsim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
jsbsim.srcs += downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c
+jsbsim.srcs += subsystems/settings.c
+jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
+
######################################################################
##
## Final Target Allocations
diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
new file mode 100644
index 00000000000..87d72683c7e
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
@@ -0,0 +1,5 @@
+
+
+include $(CFG_FIXEDWING)/imu_ppzuav.makefile
+
+ap.CFLAGS += -DASPIRIN_IMU
diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
index e6ad633f17a..c6ee27d0c1f 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
@@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
#ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2100 not used on this imu
#else ifeq ($(ARCH), stm32)
#imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
#imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
new file mode 100644
index 00000000000..8f224a91668
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
@@ -0,0 +1,22 @@
+
+IMU_PPZUAVIMU_CFLAGS = -DUSE_IMU
+IMU_PPZUAVIMU_CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
+
+IMU_PPZUAVIMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_MODULES)/ins/ins_ppzuavimu.c
+
+
+IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C
+ifdef STM32
+ IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C2
+ IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2
+else
+ IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C0
+ IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0
+endif
+
+ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS)
+ap.srcs += $(IMU_PPZUAVIMU_SRCS)
+
+ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
+
diff --git a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
index e33a41c25ed..dc36fad9562 100644
--- a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
+++ b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
@@ -15,5 +15,5 @@ ifeq ($(NORADIO), False)
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/rc_datalink.c
# arch only with sim target for compatibility (empty functions)
- sim.srcs += $(SRC_ARCH)/radio_control/rc_datalink.c
+ sim.srcs += $(SRC_ARCH)/subsystems/radio_control/rc_datalink.c
endif
diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile
index bda8b08365d..3c3feb18fa9 100644
--- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile
+++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile
@@ -10,9 +10,10 @@ ifdef AHRS_ALIGNER_LED
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
-AHRS_SRCS += subsystems/ahrs.c
-AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl.c
-AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
+AHRS_SRCS += subsystems/ahrs.c
+AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl.c
+AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
+AHRS_SRCS += math/pprz_trig_int.c
ap.CFLAGS += $(AHRS_CFLAGS)
ap.srcs += $(AHRS_SRCS)
@@ -20,3 +21,23 @@ ap.srcs += $(AHRS_SRCS)
sim.CFLAGS += $(AHRS_CFLAGS)
sim.srcs += $(AHRS_SRCS)
+
+# Extra stuff for fixedwings
+
+ifdef CPU_LED
+ ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+ AHRS_PROPAGATE_FREQUENCY = 60
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+ AHRS_CORRECT_FREQUENCY = 60
+endif
+
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+
diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example
index e9a2ea99a18..b54fddac02a 100644
--- a/conf/control_panel.xml.example
+++ b/conf/control_panel.xml.example
@@ -53,6 +53,10 @@
+
+
+
+
@@ -63,6 +67,11 @@
+
+
+
+
+
diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
new file mode 100644
index 00000000000..a30a00ba30e
--- /dev/null
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -0,0 +1,165 @@
+
+
+
+
+
+
+#include "datalink.h"
+/*
+#ifndef VARDECLARED
+#define VARDECLARED
+float varsweepsize=110;
+#endif
+*/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/ground_segment/joystick/attack3_booz_nav.xml b/conf/joystick/attack3_booz_nav.xml
similarity index 73%
rename from sw/ground_segment/joystick/attack3_booz_nav.xml
rename to conf/joystick/attack3_booz_nav.xml
index 583f3171023..fbeb7114ee2 100644
--- a/sw/ground_segment/joystick/attack3_booz_nav.xml
+++ b/conf/joystick/attack3_booz_nav.xml
@@ -11,8 +11,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
diff --git a/conf/joystick/aurora_skate_controller.xml b/conf/joystick/aurora_skate_controller.xml
new file mode 100644
index 00000000000..e803f2707fd
--- /dev/null
+++ b/conf/joystick/aurora_skate_controller.xml
@@ -0,0 +1,144 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/cockpit_sx.xml b/conf/joystick/cockpit_sx.xml
new file mode 100644
index 00000000000..621b179458a
--- /dev/null
+++ b/conf/joystick/cockpit_sx.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/ikarus_usb.xml b/conf/joystick/ikarus_usb.xml
new file mode 100644
index 00000000000..1f5bb2a379a
--- /dev/null
+++ b/conf/joystick/ikarus_usb.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/logitech_dual_action.xml b/conf/joystick/logitech_dual_action.xml
new file mode 100644
index 00000000000..e9d7572d7ca
--- /dev/null
+++ b/conf/joystick/logitech_dual_action.xml
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/logitech_f710.xml b/conf/joystick/logitech_f710.xml
new file mode 100644
index 00000000000..dc6c75ef6b4
--- /dev/null
+++ b/conf/joystick/logitech_f710.xml
@@ -0,0 +1,153 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/ground_segment/joystick/ngs_set_actuators.xml b/conf/joystick/ngs_set_actuators.xml
similarity index 100%
rename from sw/ground_segment/joystick/ngs_set_actuators.xml
rename to conf/joystick/ngs_set_actuators.xml
diff --git a/sw/ground_segment/joystick/xbox_booz_nav.xml b/conf/joystick/xbox_booz_nav.xml
similarity index 100%
rename from sw/ground_segment/joystick/xbox_booz_nav.xml
rename to conf/joystick/xbox_booz_nav.xml
diff --git a/conf/messages.xml b/conf/messages.xml
index df1e53d0a7f..fdd74237358 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -37,12 +37,12 @@
-
-
-
-
-
-
+
+
+
+
+
+
@@ -443,7 +443,7 @@
-
+
@@ -794,10 +794,14 @@
-
-
-
-
+
+
+
+
+
+
+
+
@@ -1104,18 +1108,18 @@
-
-
-
+
+
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
@@ -1325,7 +1329,7 @@
-
+
@@ -1563,7 +1567,14 @@
-
+
+
+
+
+
+
+
+
@@ -1596,7 +1607,12 @@
-
+
+
+
+
+
+
@@ -1942,6 +1958,15 @@
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/cartography.xml b/conf/modules/cartography.xml
new file mode 100644
index 00000000000..99e941a0f03
--- /dev/null
+++ b/conf/modules/cartography.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/gps_ubx_uart.xml b/conf/modules/gps_ubx_uart.xml
new file mode 100644
index 00000000000..55ea724d27e
--- /dev/null
+++ b/conf/modules/gps_ubx_uart.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/humid_sht_i2c.xml b/conf/modules/humid_sht_i2c.xml
index 949d3acf7c4..a9587748e3d 100644
--- a/conf/modules/humid_sht_i2c.xml
+++ b/conf/modules/humid_sht_i2c.xml
@@ -10,7 +10,7 @@
-
+
diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml
index b1d4493404e..7af675ada2a 100644
--- a/conf/modules/ins_arduimu_basic.xml
+++ b/conf/modules/ins_arduimu_basic.xml
@@ -6,13 +6,13 @@
-
+
-
+
diff --git a/conf/modules/ins_aspirin_via_i2c.xml b/conf/modules/ins_aspirin_via_i2c.xml
new file mode 100644
index 00000000000..d5f93cd3273
--- /dev/null
+++ b/conf/modules/ins_aspirin_via_i2c.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
new file mode 100644
index 00000000000..a2544c204df
--- /dev/null
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml
index 4990c68b8cc..6ff09118062 100644
--- a/conf/modules/ins_xsens_MTiG_fixedwing.xml
+++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml
@@ -11,12 +11,16 @@
+
-
+
+
+
+
diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml
new file mode 100644
index 00000000000..c8439d8e3e0
--- /dev/null
+++ b/conf/modules/mag_hmc5843.xml
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/R6107SP_7ch.xml b/conf/radios/R6107SP_7ch.xml
new file mode 100644
index 00000000000..1886310cfab
--- /dev/null
+++ b/conf/radios/R6107SP_7ch.xml
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml
index beafad6a5e0..8ace2c0b9ee 100644
--- a/conf/settings/tuning_ctl_new.xml
+++ b/conf/settings/tuning_ctl_new.xml
@@ -4,67 +4,27 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
+
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
@@ -77,11 +37,11 @@
-
+
-
+
@@ -114,13 +74,10 @@
-
-
-
diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index c106da4e29d..181c1f5bef9 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -28,6 +28,10 @@
+
+
+
+
@@ -50,6 +54,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml
index 4db7955d4ce..63bf030e13a 100644
--- a/conf/xsens_MTi-G.xml
+++ b/conf/xsens_MTi-G.xml
@@ -40,6 +40,11 @@
+
+
+
+
+
@@ -118,6 +123,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 248893f693f..34bedf56790 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -139,16 +139,34 @@
#define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
#ifdef IMU_TYPE_H
-#include "subsystems/imu.h"
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
-#define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+# include "subsystems/imu.h"
+# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
+# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
+# define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
+# define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
+# define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+# ifdef USE_MAGNETOMETER
+# define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
+# else
+# define PERIODIC_SEND_IMU_MAG(_chan) {}
+# endif
#else
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO(_chan) {}
+# ifdef INS_MODULE_H
+# include "modules/ins/ins_module.h"
+# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO(_chan, &ins_p, &ins_q, &ins_r)}
+# define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL(_chan, &ins_ax, &ins_ay, &ins_az)}
+# define PERIODIC_SEND_IMU_MAG(_chan) { DOWNLINK_SEND_IMU_MAG(_chan, &ins_mx, &ins_my, &ins_mz)}
+# else
+# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_ACCEL(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO(_chan) {}
+# define PERIODIC_SEND_IMU_MAG(_chan) {}
+# endif
#endif
#ifdef IMU_ANALOG
@@ -184,17 +202,15 @@
#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
#endif
-//#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */
#define PERIODIC_SEND_GPS(_chan) { \
static uint8_t i; \
- static uint8_t last_cnos[GPS_NB_CHANNELS]; \
int16_t climb = -gps.ned_vel.z; \
- int16_t course = DegOfRad(gps.course / 10); \
+ int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \
DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
- if (i == gps.nb_channels) i = 0; \
- if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
+ if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \
+ if (i >= gps.nb_channels * 2) i = 0; \
+ if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \
DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
- last_cnos[i] = gps.svinfos[i].cno; \
} \
i++; \
}
diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index f8f02fc83e8..98f65140a99 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -139,7 +139,7 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s
p->idx_buf++;
} else {
if (trans->type == I2CTransTxRx) {
- //trans->type = I2CTransRx; /* FIXME should not change type */
+ trans->type = I2CTransRx; /* FIXME should not change type */
p->idx_buf = 0;
trans->slave_addr |= 1;
I2cSendStart(p);
@@ -270,6 +270,11 @@ void i2c0_hw_init ( void ) {
#endif
#endif
+#ifndef I2C1_VIC_SLOT
+#define I2C1_VIC_SLOT 11
+#endif
+
+
void i2c1_ISR(void) __attribute__((naked));
void i2c1_ISR(void) {
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c
new file mode 100644
index 00000000000..4e401891331
--- /dev/null
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c
@@ -0,0 +1,78 @@
+#include "subsystems/imu.h"
+
+
+#include "mcu_periph/i2c.h"
+
+
+void imu_aspirin_arch_int_enable(void)
+{
+}
+
+void imu_aspirin_arch_int_disable(void)
+{
+}
+
+void imu_aspirin_arch_init(void)
+{
+}
+
+
+void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
+
+// Adxl345Select();
+// SPI_I2S_SendData(SPI2, addr);
+// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+// SPI_I2S_SendData(SPI2, val);
+// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
+// Adxl345Unselect();
+
+}
+
+void adxl345_clear_rx_buf(void) {
+}
+
+//void adxl345_start_reading_data(void) {
+//}
+
+/*
+ *
+ * Gyro data ready
+ *
+ */
+
+void exti15_10_irq_handler(void) {
+
+ /* clear EXTI */
+// if(EXTI_GetITStatus(EXTI_Line14) != RESET)
+// EXTI_ClearITPendingBit(EXTI_Line14);
+
+ imu_aspirin.gyro_eoc = TRUE;
+ imu_aspirin.status = AspirinStatusReadingGyro;
+
+}
+
+/*
+ *
+ * Accel data ready
+ *
+ */
+void exti2_irq_handler(void) {
+
+ /* clear EXTI */
+// if(EXTI_GetITStatus(EXTI_Line2) != RESET)
+// EXTI_ClearITPendingBit(EXTI_Line2);
+
+// adxl345_start_reading_data();
+
+}
+
+/*
+ *
+ * Accel end of DMA transfert
+ *
+ */
+void dma1_c4_irq_handler(void) {
+
+ imu_aspirin.accel_available = TRUE;
+}
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h
new file mode 100644
index 00000000000..4345e8b14c1
--- /dev/null
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h
@@ -0,0 +1,16 @@
+#ifndef IMU_ASPIRIN_ARCH_H
+#define IMU_ASPIRIN_ARCH_H
+
+#include "subsystems/imu.h"
+
+#include "led.h"
+
+extern void imu_aspirin_arch_init(void);
+extern void imu_aspirin_arch_int_enable(void);
+extern void imu_aspirin_arch_int_disable(void);
+extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
+extern void adxl345_clear_rx_buf(void);
+extern void adxl345_start_reading_data(void);
+
+
+#endif /* IMU_ASPIRIN_ARCH_H */
diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c
index c493cb65218..f8763b31478 100644
--- a/sw/airborne/arch/lpc21/subsystems/settings_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c
@@ -40,6 +40,10 @@
#include "subsystems/settings.h"
+#include "LPC21xx.h"
+#include BOARD_CONFIG
+#include "armVIC.h"
+
#define IAP_LOCATION 0x7FFFFFF1
#define IAP_PREPARE_SECTORS 50
diff --git a/sw/airborne/arch/lpc21/vic_slots_fw.txt b/sw/airborne/arch/lpc21/vic_slots_fw.txt
index 6c94aa7bfe4..86f238d5a80 100644
--- a/sw/airborne/arch/lpc21/vic_slots_fw.txt
+++ b/sw/airborne/arch/lpc21/vic_slots_fw.txt
@@ -17,6 +17,7 @@ SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
MICROMAG_DRDY_VIC_SLOT 9 micromag
hardcoded, no define 10 usb, e.g. telemetry_transparent_usb
hardcoded, no define 11 EXTINT in baro_scp module
+I2C1_VIC_SLOT ? (11) (11 is default in mcu_periph/i2c_arch.c)
I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista
MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1
MAX11040_DRDY_VIC_SLOT ? max11040 adc module
diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
new file mode 100644
index 00000000000..5b6e96f2d58
--- /dev/null
+++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
@@ -0,0 +1,47 @@
+/** ArduIMU simulation. OCaml binding.
+ * Sim provides IR sensor reading and airspeed
+ */
+
+
+#include
+#include "generated/airframe.h"
+
+#include
+
+#include "estimator.h"
+
+// Prevent undefined reference (from estimator.c)
+#include "subsystems/sensors/infrared.h"
+struct Infrared infrared;
+
+// Arduimu empty implementation
+#include "modules/ins/ins_arduimu_basic.h"
+
+struct FloatEulers arduimu_eulers;
+struct FloatRates arduimu_rates;
+struct FloatVect3 arduimu_accel;
+
+float ins_roll_neutral;
+float ins_pitch_neutral;
+
+void ArduIMU_init( void ) {}
+void ArduIMU_periodic( void ) {}
+void ArduIMU_periodicGPS( void ) {}
+void ArduIMU_event( void ) {}
+
+// Updates from Ocaml sim
+float sim_air_speed;
+
+value set_ir_and_airspeed(
+ value roll __attribute__ ((unused)),
+ value front __attribute__ ((unused)),
+ value top __attribute__ ((unused)),
+ value air_speed
+ ) {
+ // Feed directly the estimator
+ estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral;
+ estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral;
+ sim_air_speed = Double_val(air_speed);
+ return Val_unit;
+}
+
diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
index f50c285037d..d968eb88ada 100644
--- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
+++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
@@ -40,6 +40,7 @@ void hmc5843_arch_init( void ) {
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
+#ifdef HMC5843_USE_INT
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource5);
EXTI_InitTypeDef EXTI_InitStructure;
EXTI_InitStructure.EXTI_Line = EXTI_Line5;
@@ -54,6 +55,7 @@ void hmc5843_arch_init( void ) {
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
+#endif
}
void hmc5843_arch_reset(void)
@@ -67,5 +69,7 @@ void exti9_5_irq_handler(void) {
if(EXTI_GetITStatus(EXTI_Line5) != RESET)
EXTI_ClearITPendingBit(EXTI_Line5);
+#ifdef HMC5843_USE_INT
hmc5843.ready_for_read = TRUE;
+#endif
}
diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
index db2ff4426bf..bfb078ab1fb 100644
--- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
+++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
@@ -24,4 +24,12 @@
#ifndef HMC5843_ARCH_H
#define HMC5843_ARCH_H
+#include
+
+/* returns true if conversion done */
+static inline int mag_eoc(void)
+{
+ return GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5);
+}
+
#endif /* HMC5843_ARCH_H */
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
index 37153420091..676a890331e 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
@@ -24,11 +24,13 @@ void dma1_c4_irq_handler(void);
void imu_aspirin_arch_int_enable(void) {
NVIC_InitTypeDef NVIC_InitStructure;
+#ifdef ASPIRIN_USE_GYRO_INT
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
+#endif
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
@@ -36,16 +38,27 @@ void imu_aspirin_arch_int_enable(void) {
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
+ /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
+ NVIC_InitTypeDef NVIC_init_struct = {
+ .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+ .NVIC_IRQChannelPreemptionPriority = 0,
+ .NVIC_IRQChannelSubPriority = 0,
+ .NVIC_IRQChannelCmd = ENABLE
+ };
+ NVIC_Init(&NVIC_init_struct);
+
}
void imu_aspirin_arch_int_disable(void) {
NVIC_InitTypeDef NVIC_InitStructure;
+#ifdef ASPIRIN_USE_GYRO_INT
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
+#endif
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
@@ -53,6 +66,15 @@ void imu_aspirin_arch_int_disable(void) {
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
+ /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
+ NVIC_InitTypeDef NVIC_init_struct = {
+ .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+ .NVIC_IRQChannelPreemptionPriority = 0,
+ .NVIC_IRQChannelSubPriority = 0,
+ .NVIC_IRQChannelCmd = DISABLE
+ };
+ NVIC_Init(&NVIC_init_struct);
+
}
void imu_aspirin_arch_init(void) {
@@ -83,12 +105,14 @@ void imu_aspirin_arch_init(void) {
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
+#ifdef ASPIRIN_USE_GYRO_INT
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
+#endif
/* Accel */
/* set accel slave select as output and assert it ( on PB12) */
@@ -137,14 +161,6 @@ void imu_aspirin_arch_init(void) {
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPI2, &SPI_InitStructure);
- /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
- NVIC_InitTypeDef NVIC_init_struct = {
- .NVIC_IRQChannel = DMA1_Channel4_IRQn,
- .NVIC_IRQChannelPreemptionPriority = 0,
- .NVIC_IRQChannelSubPriority = 0,
- .NVIC_IRQChannelCmd = ENABLE
- };
- NVIC_Init(&NVIC_init_struct);
/* Enable SPI_2 DMA clock ---------------------------------------------------*/
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
@@ -233,8 +249,10 @@ void exti15_10_irq_handler(void) {
if(EXTI_GetITStatus(EXTI_Line14) != RESET)
EXTI_ClearITPendingBit(EXTI_Line14);
+#ifdef ASPIRIN_USE_GYRO_INT
imu_aspirin.gyro_eoc = TRUE;
imu_aspirin.status = AspirinStatusReadingGyro;
+#endif
}
@@ -260,6 +278,15 @@ void exti2_irq_handler(void) {
*/
void dma1_c4_irq_handler(void) {
Adxl345Unselect();
+ if (DMA_GetITStatus(DMA1_IT_TC4)) {
+ // clear int pending bit
+ DMA_ClearITPendingBit(DMA1_IT_GL4);
+
+ // mark as available
+ imu_aspirin.accel_available = TRUE;
+ }
+
+ // disable DMA Channel
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
/* Disable SPI_2 Rx and TX request */
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
@@ -268,5 +295,4 @@ void dma1_c4_irq_handler(void) {
DMA_Cmd(DMA1_Channel4, DISABLE);
DMA_Cmd(DMA1_Channel5, DISABLE);
- imu_aspirin.accel_available = TRUE;
}
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
index 4345e8b14c1..5891d37df56 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
@@ -2,8 +2,7 @@
#define IMU_ASPIRIN_ARCH_H
#include "subsystems/imu.h"
-
-#include "led.h"
+#include
extern void imu_aspirin_arch_init(void);
extern void imu_aspirin_arch_int_enable(void);
@@ -12,5 +11,11 @@ extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_clear_rx_buf(void);
extern void adxl345_start_reading_data(void);
+static inline int imu_aspirin_eoc(void)
+{
+ return !GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_14);
+}
+
+
#endif /* IMU_ASPIRIN_ARCH_H */
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 1b6f0bd08d0..106156e5649 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -41,6 +41,7 @@
#if defined NAV || defined WIND_INFO
#include "estimator.h"
+#include "subsystems/nav.h"
#endif
#ifdef USE_JOYSTICK
@@ -187,11 +188,20 @@ void dl_parse_msg(void) {
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
LED_TOGGLE(3);
- parse_rc_datalink(
+ parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else
+ if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
+LED_TOGGLE(3);
+ parse_rc_4ch_datalink(
+ DL_RC_4CH_mode(dl_buffer),
+ DL_RC_4CH_throttle(dl_buffer),
+ DL_RC_4CH_roll(dl_buffer),
+ DL_RC_4CH_pitch(dl_buffer),
+ DL_RC_4CH_yaw(dl_buffer));
+ } else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index 02ecaaf8263..fcc14344f19 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -198,7 +198,7 @@ static inline void v_ctl_set_pitch ( void ) {
if (v_ctl_auto_pitch_igain < 0.) {
v_ctl_auto_pitch_sum_err += err*(1./60.);
- BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
+ BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain));
}
// PI loop + feedforward ctl
@@ -223,7 +223,7 @@ static inline void v_ctl_set_throttle( void ) {
if (v_ctl_auto_throttle_igain < 0.) {
v_ctl_auto_throttle_sum_err += err*(1./60.);
- BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);
+ BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain));
}
// PID loop + feedforward ctl
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 653304e0007..4e98435640b 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -74,8 +74,8 @@
#ifdef USE_AHRS
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
-#include "subsystems/ahrs/ahrs_float_dcm.h"
-static inline void on_gyro_accel_event( void );
+#include AHRS_TYPE_H
+static inline void on_gyro_event( void );
static inline void on_accel_event( void );
static inline void on_mag_event( void );
#endif
@@ -269,7 +269,7 @@ static inline void telecommand_task( void ) {
#ifdef FAILSAFE_DELAY_WITHOUT_GPS
-#define GpsTimeoutError (cpu_time_sec - gps.last_msg_time > FAILSAFE_DELAY_WITHOUT_GPS)
+#define GpsTimeoutError (cpu_time_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS)
#endif
/** \fn void navigation_task( void )
@@ -398,7 +398,7 @@ static inline void attitude_loop( void ) {
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
-
+
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
@@ -410,6 +410,12 @@ static inline void attitude_loop( void ) {
}
+#ifdef USE_AHRS
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+volatile uint8_t new_ins_attitude = 0;
+#endif
+#endif
+
void periodic_task_ap( void ) {
static uint8_t _60Hz = 0;
@@ -608,7 +614,7 @@ void event_task_ap( void ) {
#endif
#ifdef USE_AHRS
- ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
+ ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif // USE_AHRS
#ifdef USE_GPS
@@ -630,7 +636,7 @@ void event_task_ap( void ) {
}
modules_event_task();
-
+
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
@@ -639,7 +645,7 @@ void event_task_ap( void ) {
new_ins_attitude = 0;
}
#endif
-
+
} /* event_task_ap() */
@@ -656,7 +662,7 @@ static inline void on_gps_solution( void ) {
static inline void on_accel_event( void ) {
}
-static inline void on_gyro_accel_event( void ) {
+static inline void on_gyro_event( void ) {
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
@@ -721,15 +727,21 @@ static inline void on_gyro_accel_event( void ) {
LED_OFF(AHRS_CPU_LED);
#endif
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+ new_ins_attitude = 1;
+#endif
+
}
-static inline void on_mag_event(void) {
- /*
+static inline void on_mag_event(void)
+{
+#ifdef IMU_MAG_X_SIGN
ImuScaleMag(imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag();
- ahrs_update_fw_estimator();
+// ahrs_update_fw_estimator();
}
- */
+#endif
}
#endif // USE_AHRS
+
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
index 3d4728d1681..9f8473b7e2f 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
@@ -31,29 +31,9 @@ float read_adc(int select)
float temp;
if (SENSOR_SIGN[select]<0){
temp = (AN_OFFSET[select]-AN[select]);
- if (abs(temp)>900) {
-#if PRINT_DEBUG != 0
- Serial.print("!!!ADC:1,VAL:");
- Serial.print (temp);
- Serial.println("***");
-#endif
-#if PERFORMANCE_REPORTING == 1
- adc_constraints++;
-#endif
- }
return constrain(temp,-900,900); //Throw out nonsensical values
} else {
temp = (AN[select]-AN_OFFSET[select]);
- if (abs(temp)>900) {
-#if PRINT_DEBUG != 0
- Serial.print("!!!ADC:2,VAL:");
- Serial.print (temp);
- Serial.println("***");
-#endif
-#if PERFORMANCE_REPORTING == 1
- adc_constraints++;
-#endif
- }
return constrain(temp,-900,900);
}
}
@@ -61,8 +41,8 @@ float read_adc(int select)
//Activating the ADC interrupts.
void Analog_Init(void)
{
- ADCSRA|=(1<=8) MuxSel=0;
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
index 305c2bac57b..ead08bda940 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
@@ -21,27 +21,13 @@ void Normalize(void)
renorm= .5 * (3-renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1
+#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???SQT:1,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???PRB:1,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
@@ -53,25 +39,11 @@ void Normalize(void)
renorm= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???SQT:2,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???PRB:2,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
@@ -83,23 +55,11 @@ void Normalize(void)
renorm= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???SQT:3,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???PRB:3,RNM:");
- Serial.print (renorm);
- Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
@@ -137,6 +97,8 @@ void Drift_correction(void)
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
+ // Set to 0 if high_accel_flag is true
+ if (high_accel_flag) Accel_weight = 0.;
#if PERFORMANCE_REPORTING == 1
tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
@@ -170,23 +132,18 @@ void Drift_correction(void)
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > ToRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
-#if PRINT_DEBUG != 0
- Serial.print("!!!INT:1,MAG:");
- Serial.print (ToDeg(Integrator_magnitude));
-
- Serial.println("***");
-#endif
}
}
+
/**************************************************/
void Accel_adjust(void)
{
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
}
-/**************************************************/
+/**************************************************/
void Matrix_update(void)
{
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
@@ -202,7 +159,6 @@ void Matrix_update(void)
Accel_adjust(); //Remove centrifugal acceleration.
-#if OUTPUTMODE==1
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
@@ -212,17 +168,6 @@ void Matrix_update(void)
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
-#else // Uncorrected data (no drift correction)
- Update_Matrix[0][0]=0;
- Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
- Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
- Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
- Update_Matrix[1][1]=0;
- Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
- Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
- Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
- Update_Matrix[2][2]=0;
-#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
@@ -235,16 +180,11 @@ void Matrix_update(void)
}
}
+/**************************************************/
void Euler_angles(void)
{
-#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
- roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
- pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
- yaw = 0;
-#else
pitch = -asin(DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
-#endif
}
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
index 6a2ddf704d4..d1850d86e96 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
@@ -25,6 +25,7 @@ void parse_pprz_gps() {
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11
stGpsFix = Paparazzi_GPS_buffer[12];
stFlags = Paparazzi_GPS_buffer[13];
+ high_accel_flag = Paparazzi_GPS_buffer[14];
if((stGpsFix >= 0x03) && (stFlags&0x01)) {
gpsFix = 0; //valid position
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
index 49170c774ca..2ee09a3d7cd 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
@@ -1,24 +1,28 @@
//*****I2C Output************************************************************
-
-void requestEvent(){
-#if PRINT_DEBUG != 0
- Serial.println("Sending IMU Data");
-#endif
-
+void fill_I2C_message() {
// Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
// Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
// Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
I2C_Message_ar[0] = int(roll*(1<<12));
I2C_Message_ar[1] = int(pitch*(1<<12));
I2C_Message_ar[2] = int(yaw*(1<<12));
- I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
- I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
- I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
+ I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12));
+ I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12));
+ I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12));
I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
+}
+
+void requestEvent(){
+#if PRINT_DEBUG != 0
+ Serial.println("Sending IMU Data");
+#endif
+
+ fill_I2C_message();
+
byte* pointer;
pointer = (byte*) &I2C_Message_ar;
Wire.send(pointer, 18);
@@ -51,6 +55,7 @@ void receiveEvent(int howMany){
void printdata(void){
#if PRINT_I2C_MSG == 1
+ fill_I2C_message();
Serial.print("Time ");
Serial.print(millis());
Serial.print("; Roll ");
@@ -89,8 +94,7 @@ void printdata(void){
Serial.print (",AN4:");
Serial.print(read_adc(4));
Serial.print (",AN5:");
- Serial.print(read_adc(5));
- Serial.print (",");
+ Serial.println(read_adc(5));
#endif
#if PRINT_DCM == 1
@@ -111,8 +115,7 @@ void printdata(void){
Serial.print (",EX7:");
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
Serial.print (",EX8:");
- Serial.print(convert_to_dec(DCM_Matrix[2][2]));
- Serial.print (",");
+ Serial.println(convert_to_dec(DCM_Matrix[2][2]));
#endif
#if PRINT_EULER == 1
@@ -123,8 +126,7 @@ void printdata(void){
Serial.print(",YAW:");
Serial.print(ToDeg(yaw));
Serial.print(",IMUH:");
- Serial.print((imu_health>>8)&0xff);
- Serial.print (",");
+ Serial.println((imu_health>>8)&0xff);
#endif
#if PRINT_GPS == 1
@@ -141,6 +143,7 @@ void printdata(void){
gps_messages_sent++;
#endif
}
+ Serial.println("");
#endif
}
@@ -174,7 +177,7 @@ void printPerfData(long time)
Serial.print(gps_messages_sent,DEC);
Serial.print(",imu:");
Serial.print((imu_health>>8),DEC);
- Serial.print(",***");
+ Serial.println(",***");
// Reset counters
mainLoop_count = 0;
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
index df41fd57789..b213b961b03 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
@@ -30,9 +30,6 @@
/*For debugging propurses*/
#define PRINT_DEBUG 0 //Will print Debug messages
-//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
-#define OUTPUTMODE 1
-
#define PRINT_I2C_MSG 0 //Will print the I2C output buffer
#define PRINT_DCM 0 //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
@@ -49,7 +46,7 @@ int I2C_Message_ar[9];
// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
-#define PRINT_BINARY 0 //Will print binary message and suppress ASCII messages (above)
+#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above)
// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others
#define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min
@@ -70,10 +67,9 @@ int I2C_Message_ar[9];
#define ToDeg(x) (x*57.2957795131) // *180/pi
// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
-// Tested values : 0.96,0.96,0.94
-#define Gyro_Gain_X 0.92 //X axis Gyro gain
-#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
+#define Gyro_Gain_X 1.0 //X axis Gyro gain
+#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
+#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
@@ -84,7 +80,7 @@ int I2C_Message_ar[9];
//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
#define Ki_YAW 0.00005
-#define ADC_WARM_CYCLES 75
+#define ADC_WARM_CYCLES 100
#define FALSE 0
#define TRUE 1
@@ -108,11 +104,7 @@ float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};
-//Magnetometer variables
-//int magnetom_x;
-//int magnetom_y;
-//int magnetom_z;
-//float MAG_Heading;
+boolean high_accel_flag = false; // disable update on accelerometers if true
// Euler angles
float roll;
@@ -268,7 +260,10 @@ void loop() //Main Loop
{
timeNow = millis();
- if((timeNow-timer)>=20) // Main loop runs at 50Hz
+ // 20 -> Main loop runs at 50Hz
+ // 5 -> Main loop runs at 200Hz
+ // Max measured duty time around 3ms
+ if((timeNow-timer)>=5)
{
timer_old = timer;
timer = timeNow;
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
index 012b2402a0a..b158fae8255 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
@@ -38,6 +38,19 @@
#define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
#endif
+#if defined (SUPERVISION_MAX_NEGATIVE_MOTOR_STEP) || defined (SUPERVISION_MAX_POSITIVE_MOTOR_STEP)
+#define SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
+
+#ifndef SUPERVISION_MAX_NEGATIVE_MOTOR_STEP
+#define SUPERVISION_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
+#endif
+/*
+#ifndef SUPERVISION_MAX_POSITIVE_MOTOR_STEP
+#define SUPERVISION_MAX_POSITIVE_MOTOR_STEP INT32_MAX
+#endif
+*/
+#endif
+
static const int32_t roll_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_ROLL_COEF;
static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_PITCH_COEF;
static const int32_t yaw_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_YAW_COEF;
@@ -72,6 +85,31 @@ __attribute__ ((always_inline)) static inline void bound_commands(void) {
SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
}
+#ifdef SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
+__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
+ uint8_t j;
+ static int32_t prev_commands[SUPERVISION_NB_MOTOR];
+ static uint8_t initialized = 0;
+
+ if (initialized == 1) {
+ for (j=0; j
-
-#include "gps.h"
-#include "sys_time.h"
-#include "generated/airframe.h"
-
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-uint8_t gps_mode;
-uint16_t gps_week;
-uint32_t gps_itow;
-int32_t gps_alt;
-uint16_t gps_gspeed;
-int16_t gps_climb;
-int16_t gps_course;
-int32_t gps_utm_east, gps_utm_north;
-uint8_t gps_utm_zone;
-int32_t gps_lat, gps_lon;
-uint16_t gps_PDOP;
-uint32_t gps_Pacc, gps_Sacc;
-uint8_t gps_numSV;
-uint8_t gps_configuring;
-
-uint16_t gps_reset;
-
-void reset_gps_watchdog(void)
-{
- last_gps_msg_t = cpu_time_sec;
-}
-
-volatile bool_t gps_msg_received;
-bool_t gps_pos_available;
-uint8_t gps_nb_ovrn;
-
-uint8_t gps_nb_channels;
-
-struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-
-
-void gps_init( void ) {}
-void gps_configure( void ) {}
-void parse_gps_msg( void ) {}
-void gps_configure_uart( void ) {}
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 2a0e34b4393..6fd09ba4773 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -522,7 +522,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
FLOAT_QUAT_NORMALIZE(_a2c); \
}
-/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
+/* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \
(_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
(_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
@@ -530,7 +530,8 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
(_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \
}
-#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
+/* Quaternion multiplication q_a2c = q_b2c * q_a2b */
+#define FLOAT_QUAT_MULT(_a2c, _b2c, _a2b) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 0a410b2e6a5..f8cc0c926a5 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -182,6 +182,7 @@ struct Int64Vect3 {
};
+// Real (floating point) -> Binary Fixed Point (int)
#define BFP_OF_REAL(_vr, _frac) ((_vr)*(1<<(_frac)))
#define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
#define RATE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_RATE_FRAC)
diff --git a/sw/airborne/math/pprz_geodetic_wgs84.h b/sw/airborne/math/pprz_geodetic_wgs84.h
new file mode 100644
index 00000000000..640bd7be309
--- /dev/null
+++ b/sw/airborne/math/pprz_geodetic_wgs84.h
@@ -0,0 +1,71 @@
+#ifndef PPRZ_GEODETIC_WGS84
+#define PPRZ_GEODETIC_WGS84
+
+/*
+
+Ten by Ten Degree WGS-84 Geoid Heights from -180 to +170 Degrees of Longitude
+
+Geoid height approximations in meters
+
+Source:
+Defense Mapping Agency. 12 Jan 1987. GPS UE Relevant WGS-84 Data Base Package. Washington, DC: Defense Mapping Agency
+
+Link:
+http://www.colorado.edu/geography/gcraft/notes/datum/geoid84.html
+
+rows are from -180 to +170 starting north +90 to south-90
+
+
+*/
+
+
+#define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)])
+
+#define WGS84_ELLIPSOID_TO_GEOID(_Lat,_Lon,_diff) { \
+ float x = (180.0f + DegOfRad(_Lon)) / 10.0f; \
+ Bound(x,0.0f,35.99999f); \
+ float y = (90.0f - DegOfRad(_Lat)) / 10.0f; \
+ Bound(y,0.0f,17.99999f); \
+ uint8_t ex1 = (uint8_t) x; \
+ uint8_t ex2 = ex1 + 1; \
+ if (ex2 >= 36) ex2 = 0; \
+ uint8_t ey1 = (uint8_t) y; \
+ uint8_t ey2 = ey1 + 1; \
+ float lin_x = x - ((float) ex1); \
+ float lin_y = y - ((float) ey1); \
+ float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1,ey1); \
+ float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2,ey1); \
+ float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1,ey2); \
+ float h22 = lin_x * lin_y * WGS84_H(ex2,ey2); \
+ _diff = h11 + h12 + h21 + h22; \
+}
+
+
+
+
+const int8_t pprz_geodetic_wgs84_int[19][36] =
+{
+ {13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13},
+ {3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1},
+ {2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4},
+ {2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6},
+ {-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2},
+ {-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11},
+ {-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6},
+ {5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10},
+ {13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26},
+ {22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32},
+ {36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52},
+ {51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63},
+ {46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45},
+ {21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20},
+ {-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10},
+ {-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43},
+ {-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59},
+ {-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52},
+ {-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30}
+};
+
+
+
+#endif
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
new file mode 100644
index 00000000000..4dc179586db
--- /dev/null
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -0,0 +1,729 @@
+/*
+ * $Id: cartography.c 2011-04-20 10:43:13Z $
+ *
+ * Copyright (C) 2011 Vandeportaele Bertrand
+ *
+ * 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 cartography.c
+ * \brief Navigation functions for cartography of the ground
+ *
+ */
+
+
+
+
+#include "estimator.h"
+#include "stdio.h"
+
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+
+#include "std.h" //macros pas mal dans sw/include
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+//for fast debbuging, the simulation can be accelerated using the gaia software from an xterm console
+// /home/bvdp/paparazzi3/sw/simulator/gaia
+////////////////////////////////////////////////////////////////////////////////////////////////
+// for explanations about debugging macros:
+//http://gcc.gnu.org/onlinedocs/cpp/Stringification.html#Stringification
+
+// Be carefull not to use printf function in ap compilation, only use it in sim compilation
+// the DEBUG_PRINTF should be defined only in the sim part of the makefile airframe file
+#ifdef DEBUG_PRINTF
+int CPTDEBUG=0;
+#define PRTDEB(TYPE,EXP) \
+printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++;
+#define PRTDEBSTR(EXP) \
+printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++;
+#else
+#define PRTDEB(TYPE,EXP) \
+;
+
+#define PRTDEBSTR(EXP) \
+;
+#endif
+
+/*
+ exemple of use for theese macros
+ PRTDEBSTR(Init polysurvey)
+ PRTDEB(u,SurveySize)
+
+ PRTDEB(lf,PolygonCenter.x)
+ PRTDEB(lf,PolygonCenter.y)
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define DISTXY(P1X,P1Y,P2X,P2Y)\
+(sqrt( ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) ) ) )
+
+#define NORMXY(P1X,P1Y)\
+(sqrt( ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) ) ) )
+
+
+//max distance between the estimator position and an objective points to consider that the objective points is atteined
+
+#define DISTLIMIT 30
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+uint16_t railnumberSinceBoot=1; //used to count the number of rails the plane has achieved since the boot, to number the sequences of saved images
+//the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+#define USE_ONBOARD_CAMERA
+
+#ifdef USE_ONBOARD_CAMERA
+uint16_t camera_snapshot_image_number=0;
+#endif
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t survey_losange_uturn;//this flag indicates if the aircraft is turning between 2 rails (1) or if it is flying straight forward in the rail direction (0)
+
+int railnumber; //indicate the number of the rail being acquired
+int numberofrailtodo;
+
+float distrail; //distance between adjacent rails in meters, the value is set in the init function
+float distplus; //distance that the aircraft should travel before and after a rail before turning to the next rails in meters, the value is set in the init function
+
+float distrailinteractif=60; //a cheangable value that can be set interactively in the GCS, not used at that time, it can be used to choose the right value while the aircraft is flying
+
+
+static struct point point1,point2,point3; // 3 2D points used for navigation
+static struct point pointA,pointB,pointC; // 3 2D points used for navigation
+static struct point vec12,vec13;
+float norm12,norm13; // norms of 12 & 13 vectors
+
+
+
+
+float tempx,tempy; //temporary points for exchanges
+float angle1213; //angle between 12 & 13 vectors
+float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails
+
+float tempcircleradius;// used to compute the radius of the UTURN after a rail
+float circleradiusmin=40;
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+float normBM,normAM,distancefromrail;
+
+
+float course_next_rail;
+float angle_between;
+
+bool_t ProjectionInsideLimitOfRail;
+
+
+
+
+#include "modules/cartography/cartography.h"
+#include "generated/modules.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "downlink.h"
+#include "mcu_periph/uart.h"
+#include "std.h"
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+void init_carto(void) {
+}
+
+void periodic_downlink_carto(void) {
+ DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel,&camera_snapshot_image_number);
+}
+
+void start_carto(void) {
+}
+
+void stop_carto(void) {
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Inc_railnumberSinceBoot(void)
+{
+ railnumberSinceBoot++;
+ return FALSE;
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Snapshoot(void)
+{
+ camera_snapshot_image_number=railnumberSinceBoot;
+ PRTDEBSTR(SNAPSHOT)
+ cartography_periodic_downlink_carto_status = MODULES_START;
+ return FALSE;
+
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Snapshoot_Continu(void)
+{
+ camera_snapshot_image_number=railnumberSinceBoot;
+ PRTDEBSTR(SNAPSHOT)
+ cartography_periodic_downlink_carto_status = MODULES_START;
+ return TRUE;
+
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_StopSnapshoot(void)
+{
+ camera_snapshot_image_number=0;
+ PRTDEBSTR(STOP SNAPSHOT)
+ cartography_periodic_downlink_carto_status = MODULES_START;
+ return FALSE;
+
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 )
+{
+ waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x;
+ waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y;
+
+ PRTDEBSTR(nav_survey_computefourth_corner)
+ PRTDEB(f,waypoints[wp4].x)
+ PRTDEB(f,waypoints[wp4].y)
+ return FALSE;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf);
+
+
+bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf)
+//return if the projection of the estimator on the AB line is located inside the AB interval
+{
+ float a,b,c,xa,xb,xc,ya,yb,yc;
+ float f;
+ float AA1;
+ float BB1;
+ float YP;
+ float XP;
+
+ float AMx,AMy,BMx,BMy;
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+
+
+
+ xb=pointAf.x;
+ yb=pointAf.y;
+
+ xc=pointBf.x;
+ yc=pointBf.y;
+
+ xa=estimator_xf;
+ ya=estimator_yf;
+
+ //calcul des parametres de la droite pointAf pointBf
+ a = yc - yb;
+ b = xb - xc;
+ c = (yb - yc) * xb + (xc - xb) * yb ;
+
+ //calcul de la distance de la droite à l'avion
+
+
+ if (fabs(a)>1e-10)
+ *distancefromrailf = fabs((a * xa + b * ya + c) / sqrt(a * a + b * b)); //denominateur =0 iniquement si a=b=0 //peut arriver si 2 waypoints sont confondus
+ else
+ return 0;
+
+ PRTDEB(f,a)
+ PRTDEB(f,b)
+ PRTDEB(f,c)
+ PRTDEB(f,*distancefromrailf)
+
+
+ // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
+ AA1 = (xc - xb);
+ BB1 = (yc - yb);
+ if (fabs(AA1)>1e-10)
+ {
+ f=(b - (a * BB1 / AA1));
+ if (fabs(f)>1e-10)
+ YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f;
+ else
+ return 0;
+ }
+ else
+ return 0;
+
+
+
+
+ XP = (-c - b * YP) / a ; //a !=0 deja testé avant
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+ //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+
+ PRTDEB(f,AA1)
+ PRTDEB(f,BB1)
+ PRTDEB(f,YP)
+ PRTDEB(f,XP)
+
+ AMx=XP-pointAf.x;
+ AMy=YP-pointAf.y;
+ BMx=XP-pointBf.x;
+ BMy=YP-pointBf.y;
+
+ *normAMf=NORMXY(AMx,AMy);
+ *normBMf=NORMXY(BMx,BMy);
+
+ PRTDEB(f,*normAMf)
+ PRTDEB(f,*normBMf)
+
+ if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y))
+ {
+ PRTDEBSTR(NOT INSIDE)
+ return 0;
+ }
+ else
+ {
+ PRTDEBSTR(INSIDE)
+ return 1;
+ }
+}
+///////////////////////////////////////////////////////////////////////////
+//if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3,
+//This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
+bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
+{
+ //PRTDEBSTR(nav_survey_losange_carto_init)
+ survey_losange_uturn=FALSE;
+
+
+ point1.x=waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type
+ point1.y=waypoints[wp1].y;
+ point2.x=waypoints[wp2].x;
+ point2.y=waypoints[wp2].y;
+ point3.x=waypoints[wp3].x;
+ point3.y=waypoints[wp3].y;
+
+ PRTDEB(u,wp1)
+ PRTDEB(f,point1.x)
+ PRTDEB(f,point1.y)
+
+ PRTDEB(u,wp2)
+ PRTDEB(f,point2.x)
+ PRTDEB(f,point2.y)
+
+ PRTDEB(u,wp3)
+ PRTDEB(f,point3.x)
+ PRTDEB(f,point3.y)
+
+
+
+ vec12.x=point2.x-point1.x;
+ vec12.y=point2.y-point1.y;
+ PRTDEB(f,vec12.x)
+ PRTDEB(f,vec12.y)
+
+ //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
+ norm12=NORMXY(vec12.x,vec12.y);
+
+ PRTDEB(f,norm12)
+
+
+ vec13.x=point3.x-point1.x;
+ vec13.y=point3.y-point1.y;
+ PRTDEB(f,vec13.x)
+ PRTDEB(f,vec13.y)
+
+ norm13=NORMXY(vec13.x,vec13.y);
+ PRTDEB(f,norm13)
+
+ //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
+ // return FALSE;
+
+
+ if (fabs(distrailinit)<=1)
+ { //is distrailinit==0, then the aircraft should do 2 passes to register the bands
+ distrail=norm13;
+ numberofrailtodo=1;
+ }
+ else
+ {//no, so normal trajectory
+ distrail=fabs(distrailinit);
+ numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer
+ }
+
+ distplus=fabs(distplusinit);
+
+
+ PRTDEB(f,distrail)
+ PRTDEB(f,distplus)
+ PRTDEB(d,numberofrailtodo)
+ PRTDEB(d,railnumber)
+ PRTDEB(d,railnumberSinceBoot)
+
+ railnumber=-1; // the state is before the first rail, which is numbered 0
+
+ if (norm12<1e-15)
+ return FALSE;
+ if (norm13<1e-15)
+ return FALSE;
+
+
+ angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors
+
+ angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x);
+ while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI;
+ while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI;
+
+ PRTDEB(f,angle1213)
+
+ if (angle1213 >0)
+ signforturn=-1;
+ else
+ signforturn=1;
+
+
+ return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed
+ //dans le flight_plan.h
+ // if (! (nav_survey_losange_carto()))
+ // NextStageAndBreak();
+}
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_losange_carto(void)
+{
+ //test pour modifier en vol la valeur distrail
+
+ //distrail=distrailinteractif;
+
+
+ //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
+ //if the aircraft is inside the region to map, camera_snapshot_image_number will be equal to the number of rail since the last boot (not since the nav_survey_losange_carto_init, in order to get different values for differents calls to the cartography function (this number is used to name the images on the hard drive
+ camera_snapshot_image_number=0;
+
+
+ PRTDEB(f,distrail)
+
+
+
+ PRTDEBSTR(nav_survey_losange_carto)
+ PRTDEB(d,railnumber)
+
+ PRTDEB(d,railnumberSinceBoot)
+
+ //PRTDEB(f,estimator_x)
+ //PRTDEB(f,estimator_y)
+
+ //sortir du bloc si données abhérantes
+ if (norm13<1e-15)
+ {
+ PRTDEBSTR(norm13<1e-15)
+ return FALSE;
+ }
+ if (norm12<1e-15)
+ {
+ PRTDEBSTR(norm13<1e-15)
+ return FALSE;
+ }
+ if (distrail<1e-15)
+ {
+ PRTDEBSTR(distrail<1e-15)
+ return FALSE;
+ }
+
+ if (survey_losange_uturn==FALSE)
+ {
+
+ if (railnumber==-1)
+ { //se diriger vers le début du 1°rail
+ PRTDEBSTR(approche debut rail 0)
+ pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
+ pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
+
+
+ pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
+ pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
+
+ //PRTDEB(f,pointA.x)
+ //PRTDEB(f,pointA.y)
+
+
+ //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
+ //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT)
+ //if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >DISTLIMIT)
+
+
+ nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+
+ if ((DISTXY(estimator_x,estimator_y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+ {
+ nav_route_xy(estimator_x, estimator_y,pointA.x,pointA.y);
+ //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
+ }
+ else
+ {
+ PRTDEBSTR(debut rail 0)
+ //un fois arrivé, on commence le 1° rail;
+ railnumber=0;
+ railnumberSinceBoot++;
+
+ }
+ }
+
+
+ if (railnumber>=0)
+ {
+ pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+
+ pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+
+
+
+
+
+ if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
+ {
+ //rien a faire
+ }
+ else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
+ {
+ //echange pointA et B
+ tempx=pointA.x;
+ tempy=pointA.y;
+ pointA.x=pointB.x;
+ pointA.y=pointB.y;
+ pointB.x=tempx;
+ pointB.y=tempy;
+
+ }
+
+ // PRTDEB(f,pointA.x)
+ // PRTDEB(f,pointA.y)
+ // PRTDEB(f,pointB.x)
+ // PRTDEB(f,pointB.y)
+ ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+
+
+
+ // if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) >DISTLIMIT) &&
+ // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+
+
+ if (! ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
+ // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+ {
+ nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+ PRTDEBSTR(NAVROUTE)
+
+
+ //est ce que l'avion est dans la zone ou il doit prendre des images?
+ //DEJA APPELE AVANT LE IF
+ // nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+
+ if ( (normAM> distplus) && (normBM> distplus) && (distancefromrailnumberofrailtodo)
+ {
+ PRTDEBSTR(fin nav_survey_losange_carto)
+ return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
+ }
+
+ }
+ }
+ else // (survey_losange_uturn==TRUE)
+ {
+
+
+ if (distrail<200)
+ {
+ //tourne autour d'un point à mi chemin entre les 2 rails
+
+ //attention railnumber a été incrémenté en fin du rail précédent
+
+ if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
+ {
+ PRTDEBSTR(UTURN-IMPAIR)
+ //fin du rail précédent
+ pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+ pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+ //début du rail suivant
+ pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+ //milieu
+ waypoints[0].x=(pointA.x+pointB.x)/2;
+ waypoints[0].y=(pointA.y+pointB.y)/2;
+
+ tempcircleradius=distrail/2;
+ if(tempcircleradius M_PI) angle_between -= 2 * M_PI;
+ while (angle_between < -M_PI) angle_between += 2 * M_PI;
+
+ angle_between= DegOfRad(angle_between);
+ PRTDEB(f,angle_between )
+ //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
+
+ NavCircleWaypoint(0,signforturn*tempcircleradius);
+ if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) )
+ {
+ //l'avion fait le rail suivant
+ survey_losange_uturn=FALSE;
+ PRTDEBSTR(FIN UTURN-IMPAIR)
+ }
+ }
+ else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
+ {
+ PRTDEBSTR(UTURN-PAIR)
+ //fin du rail précédent
+ pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+ pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+ //début du rail suivant
+ pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+ //milieu
+ waypoints[0].x=(pointA.x+pointB.x)/2;
+ waypoints[0].y=(pointA.y+pointB.y)/2;
+
+ tempcircleradius=distrail/2;
+ if(tempcircleradius M_PI) angle_between -= 2 * M_PI;
+ while (angle_between < -M_PI) angle_between += 2 * M_PI;
+
+ angle_between= DegOfRad(angle_between);
+ PRTDEB(f,angle_between )
+ //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
+
+ NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius);
+ if (( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) -10 && angle_between< 10) )
+ {
+ //l'avion fait le rail suivant
+ survey_losange_uturn=FALSE;
+ PRTDEBSTR(FIN UTURN-PAIR)
+ }
+ }
+ }
+ else
+ { //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
+
+ if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
+ {
+ PRTDEBSTR(TRANSIT-IMPAIR)
+ //fin du rail précédent
+ pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+ pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+ //début du rail suivant
+ pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+ pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+ nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+ if ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) true
+}
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+
diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h
new file mode 100644
index 00000000000..bbbaa843764
--- /dev/null
+++ b/sw/airborne/modules/cartography/cartography.h
@@ -0,0 +1,51 @@
+/** Automatic survey of an oriented rectangle (defined by three points) */
+
+#ifndef CARTOGRAPHY_H
+#define CARTOGRAPHY_H
+
+
+
+
+void init_carto(void);
+void periodic_downlink_carto(void);
+void start_carto(void);
+void stop_carto(void);
+
+/*
+ typedef enum {NS, WE} survey_orientation_t;
+ */
+
+
+#ifdef USE_ONBOARD_CAMERA
+extern bool_t CAMERA_SNAPSHOT_REQUIERED;
+extern uint16_t camera_snapshot_image_number;
+#endif
+
+extern float distrailinteractif; //pour exporter la variable et pouvoir la changer depuis settings
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+extern bool_t nav_survey_Inc_railnumberSinceBoot(void);
+extern bool_t nav_survey_Snapshoot(void);
+bool_t nav_survey_Snapshoot_Continu(void);
+extern bool_t nav_survey_StopSnapshoot(void);
+extern bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 );
+
+extern bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrail, float distplus);
+
+extern bool_t nav_survey_losange_carto(void); // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype
+
+//(uint8_t wp1, uint8_t wp2, uint8_t wp3);
+
+/*
+ #define NavSurveylosange_cartoInit(_wp1, _wp2, _grid, _distplus) nav_survey_losange_init(_wp1, _wp2, _wp3, _grid, _distplus)
+ #define NavSurveylosange_carto nav_survey_losange
+ */
+
+
+
+#endif
+
diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/modules/gps/gps_ubx_uart.h
similarity index 64%
rename from sw/airborne/gps_xsens.h
rename to sw/airborne/modules/gps/gps_ubx_uart.h
index 2395793e5a0..3349680f443 100644
--- a/sw/airborne/gps_xsens.h
+++ b/sw/airborne/modules/gps/gps_ubx_uart.h
@@ -1,7 +1,5 @@
/*
- * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $
- *
- * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2011 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -22,27 +20,15 @@
*
*/
-/** \file gps_xsens.h
- * \brief XSens GPS
- *
-*/
-
-
-#ifndef XSENS_GPS_H
-#define XSENS_GPS_H
-
-extern uint16_t gps_reset;
-
-#define GPS_NB_CHANNELS 16
-
-extern void reset_gps_watchdog(void);
-
+/*
+ * Wrapper for UBX gps on uart
+ */
-#define GpsFixValid() (gps_mode > 0)
+#ifndef MODULE_GPS_UBX_UART_H
+#define MODULE_GPS_UBX_UART_H
-#define gps_xsens_Reset(_val) { \
- gps_reset = _val; \
-}
+#include "subsystems/gps.h"
+#include "subsystems/gps/gps_ubx.h"
+#endif // MODULE_GPS_UBX_UART_H
-#endif
diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h
index 3e1434daaa7..6fade8a21e0 100644
--- a/sw/airborne/modules/ins/imu_chimu.h
+++ b/sw/airborne/modules/ins/imu_chimu.h
@@ -93,11 +93,7 @@ typedef struct {
unsigned char m_MsgLen;
unsigned char m_TempDeviceID;
unsigned char m_DeviceID;
-<<<<<<< HEAD
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
-=======
- unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
->>>>>>> paparazzi/CHIMU
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data
CHIMU_attitude_data m_attitude;
CHIMU_attitude_data m_attrates;
diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c
index cc10e38d0c8..dbea8c92f2d 100644
--- a/sw/airborne/modules/ins/ins_arduimu.c
+++ b/sw/airborne/modules/ins/ins_arduimu.c
@@ -14,6 +14,9 @@ Autoren@ZHAW: schmiemi
#include "estimator.h"
// für das Senden von GPS-Daten an den ArduIMU
+#ifndef UBX
+#error "currently only compatible with uBlox GPS modules"
+#endif
#include "subsystems/gps.h"
int32_t GPS_Data[14];
@@ -87,11 +90,11 @@ void ArduIMU_periodicGPS( void ) {
GPS_Data [6] = gps.gspeed; //ground speed
GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs
//status
- GPS_Data [8] = gps_mode; //fix
- GPS_Data [9] = gps_status_flags; //flags
+ GPS_Data [8] = gps.fix; //fix
+ GPS_Data [9] = gps_ubx.status_flags; //flags
//sol
GPS_Data [10] = gps.fix; //fix
- //GPS_Data [11] = gps_sol_flags; //flags
+ //GPS_Data [11] = gps_ubx.sol_flags; //flags
GPS_Data [12] = -gps.ned_vel.z;
GPS_Data [13] = gps.num_sv;
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
index 1ff1c7151ad..0352c58b2d0 100644
--- a/sw/airborne/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -13,8 +13,15 @@ Autoren@ZHAW: schmiemi
// test
#include "estimator.h"
-// für das Senden von GPS-Daten an den ArduIMU
-#include "gps.h"
+// GPS data for ArduIMU
+#ifndef UBX
+#error "currently only compatible with uBlox GPS modules"
+#endif
+#include "subsystems/gps.h"
+
+// Command vector for thrust
+#include "generated/airframe.h"
+#include "inter_mcu.h"
#define NB_DATA 9
@@ -24,7 +31,7 @@ Autoren@ZHAW: schmiemi
// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
// einzugebende Adresse im ArduIMU ist 0000 1011
-//da ArduIMU das Read/Write Bit selber anfügt.
+// da ArduIMU das Read/Write Bit selber anfügt.
#define ArduIMU_SLAVE_ADDR 0x22
#ifndef DOWNLINK_DEVICE
@@ -47,6 +54,11 @@ struct FloatVect3 arduimu_accel;
float ins_roll_neutral;
float ins_pitch_neutral;
+// High Accel Flag
+#define HIGH_ACCEL_LOW_SPEED 2.0
+#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
+bool_t high_accel_flag;
+
void ArduIMU_init( void ) {
FLOAT_EULERS_ZERO(arduimu_eulers);
FLOAT_RATES_ZERO(arduimu_rates);
@@ -57,6 +69,8 @@ void ArduIMU_init( void ) {
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+
+ high_accel_flag = FALSE;
}
#define FillBufWith32bit(_buf, _index, _value) { \
@@ -70,12 +84,22 @@ void ArduIMU_periodicGPS( void ) {
if (ardu_gps_trans.status != I2CTransDone) { return; }
- FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
- FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed
- FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course
- ardu_gps_trans.buf[12] = gps_mode; // status gps fix
- ardu_gps_trans.buf[13] = gps_status_flags; // status flags
- I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
+ // Test for high acceleration:
+ // - low speed
+ // - high thrust
+ if (gps_speed_3d < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE]) {
+ high_accel_flag = TRUE;
+ } else {
+ high_accel_flag = FALSE;
+ }
+
+ FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
+ FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed
+ FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6)); // course
+ ardu_gps_trans.buf[12] = gps.fix; // status gps fix
+ ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags
+ ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
+ I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
}
@@ -132,7 +156,7 @@ void ArduIMU_event( void ) {
estimator_p = arduimu_rates.p;
ardu_ins_trans.status = I2CTransDone;
- RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
+ //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
}
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
index 82bb0622a95..1c82fae3816 100644
--- a/sw/airborne/modules/ins/ins_chimu_spi.c
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -27,7 +27,6 @@ C code to connect a CHIMU using uart
#include "ins_module.h"
#include "imu_chimu.h"
-
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
@@ -37,15 +36,13 @@ volatile uint8_t new_ins_attitude;
void ins_init( void )
{
-// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
- uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
+ uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
+// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12);
-
-
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -58,6 +55,7 @@ void ins_init( void )
{
InsSend1(quaternions[i]);
}
+
// Wait a bit (SPI send zero)
InsSend1(0);
InsSend1(0);
@@ -73,9 +71,6 @@ void ins_init( void )
}
-
-//float tempang = 0;
-
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
@@ -87,28 +82,12 @@ void parse_ins_msg( void )
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
- // RunOnceEvery(25, LED_TOGGLE(3) );
- // LED_TOGGLE(3);
+ RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
- if (CHIMU_DATA.m_attrates.euler.phi > M_PI)
- {
- CHIMU_DATA.m_attrates.euler.phi -= 2 * M_PI;
- }
- LED_TOGGLE(3);
-/* if (CHIMU_DATA.m_attitude.euler.phi == tempang)
- {
- LED_ON(3);
- }
- else
- {
- LED_OFF(3);
- }
- tempang = CHIMU_DATA.m_attitude.euler.phi;
-*/
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
}
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
index 1805c2ff93a..7f6453795b4 100644
--- a/sw/airborne/modules/ins/ins_chimu_uart.c
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -4,8 +4,11 @@ C code to connect a CHIMU using uart
#include
+
//#include "modules/ins/ins_chimu_uart.h"
+
+
// Output
#include "estimator.h"
@@ -31,66 +34,63 @@ INS_FORMAT ins_pitch_neutral;
volatile uint8_t new_ins_attitude;
-void ins_init( void )
+void ins_init( void )
{
-// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI
- uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
+ uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
+// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
-
+
+ CHIMU_Checksum(rate,12);
+
new_ins_attitude = 0;
-
+
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-
- CHIMU_Init(&CHIMU_DATA);
-
+
+ CHIMU_Init(&CHIMU_DATA);
+
// Quat Filter
for (int i=0;i<7;i++)
{
InsUartSend1(quaternions[i]);
}
+
+
+
+
+
+
+
+
// 50Hz
for (int i=0;i<12;i++)
{
InsUartSend1(rate[i]);
}
-
-}
-float tempang = 0;
+}
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
-
+
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
- // RunOnceEvery(25, LED_TOGGLE(3) );
- // LED_TOGGLE(3);
+ RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
-
- if (CHIMU_DATA.m_attitude.euler.phi == tempang)
- {
- LED_ON(3);
- }
- else
- {
- LED_OFF(3);
- }
- tempang = CHIMU_DATA.m_attitude.euler.phi;
-
+
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q);
-
+
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
}
@@ -100,7 +100,7 @@ void parse_ins_msg( void )
//Frequency defined in conf *.xml
-void ins_periodic_task( void )
+void ins_periodic_task( void )
{
// Downlink Send
}
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
new file mode 100644
index 00000000000..c0921076161
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -0,0 +1,269 @@
+/*
+ * 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
+#include "ins_ppzuavimu.h"
+#include "mcu_periph/i2c.h"
+#include "led.h"
+
+// Downlink
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+// Peripherials
+#define HMC5843_NO_IRQ
+#include "../../peripherals/itg3200.h"
+#include "../../peripherals/adxl345.h"
+#include "../../peripherals/hmc5843.h"
+
+// Results
+volatile bool_t mag_valid;
+volatile bool_t gyr_valid;
+volatile bool_t acc_valid;
+
+// Communication
+struct i2c_transaction ppzuavimu_hmc5843;
+struct i2c_transaction ppzuavimu_itg3200;
+struct i2c_transaction ppzuavimu_adxl345;
+
+// Standalone option: run module only
+#ifndef IMU_TYPE_H
+struct Imu imu;
+#endif
+
+#ifndef PERIODIC_FREQUENCY
+#define PERIODIC_FREQUENCY 60
+#endif
+
+void imu_impl_init(void)
+{
+ /////////////////////////////////////////////////////////////////////
+ // ITG3200
+ ppzuavimu_itg3200.type = I2CTransTx;
+ ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
+ ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
+#if PERIODIC_FREQUENCY == 60
+ /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
+ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
+# warning ITG3200 read at 50Hz
+#else
+# if PERIODIC_FREQUENCY == 120
+# warning ITG3200 read at 100Hz
+ /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
+ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
+# else
+# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+# endif
+#endif
+ ppzuavimu_itg3200.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+ while(ppzuavimu_itg3200.status == I2CTransPending);
+
+ /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
+ ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
+#if PERIODIC_FREQUENCY == 60
+ ppzuavimu_itg3200.buf[1] = 19; // 50Hz
+#else
+ ppzuavimu_itg3200.buf[1] = 9; // 100Hz
+#endif
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+ while(ppzuavimu_itg3200.status == I2CTransPending);
+
+ /* switch to gyroX clock */
+ ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
+ ppzuavimu_itg3200.buf[1] = 0x01;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+ while(ppzuavimu_itg3200.status == I2CTransPending);
+
+ /* no interrupts for now, but set data ready interrupt to enable reading status bits */
+ ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
+ ppzuavimu_itg3200.buf[1] = 0x01;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+ while(ppzuavimu_itg3200.status == I2CTransPending);
+
+ /////////////////////////////////////////////////////////////////////
+ // ADXL345
+
+ /* set data rate to 100Hz */
+ ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
+ ppzuavimu_adxl345.type = I2CTransTx;
+ ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
+#if PERIODIC_FREQUENCY == 60
+ ppzuavimu_adxl345.buf[1] = 0x09; // normal power and 50Hz sampling, 50Hz BW
+#else
+ ppzuavimu_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW
+#endif
+ ppzuavimu_adxl345.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+ while(ppzuavimu_adxl345.status == I2CTransPending);
+
+ /* switch to measurement mode */
+ ppzuavimu_adxl345.type = I2CTransTx;
+ ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
+ ppzuavimu_adxl345.buf[1] = 1<<3;
+ ppzuavimu_adxl345.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+ while(ppzuavimu_adxl345.status == I2CTransPending);
+
+ /* Set range to 16g but keeping full resolution of 3.9 mV/g */
+ ppzuavimu_adxl345.type = I2CTransTx;
+ ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
+ ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
+ ppzuavimu_adxl345.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+ while(ppzuavimu_adxl345.status == I2CTransPending);
+
+ /////////////////////////////////////////////////////////////////////
+ // HMC5843
+ ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
+ ppzuavimu_hmc5843.type = I2CTransTx;
+ ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
+ ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
+ ppzuavimu_hmc5843.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+ while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+ ppzuavimu_hmc5843.type = I2CTransTx;
+ ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
+ ppzuavimu_hmc5843.buf[1] = 0x01<<5;
+ ppzuavimu_hmc5843.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+ while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+ ppzuavimu_hmc5843.type = I2CTransTx;
+ ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
+ ppzuavimu_hmc5843.buf[1] = 0x00;
+ ppzuavimu_hmc5843.len_w = 2;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+ while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+}
+
+void imu_periodic( void )
+{
+ // Start reading the latest gyroscope data
+ ppzuavimu_itg3200.type = I2CTransTxRx;
+ ppzuavimu_itg3200.len_r = 9;
+ ppzuavimu_itg3200.len_w = 1;
+ ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
+
+ // Start reading the latest accelerometer data
+ ppzuavimu_adxl345.type = I2CTransTxRx;
+ ppzuavimu_adxl345.len_r = 6;
+ ppzuavimu_adxl345.len_w = 1;
+ ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
+
+ // Start reading the latest magnetometer data
+#if PERIODIC_FREQUENCY > 60
+ RunOnceEvery(2,{
+#endif
+ ppzuavimu_hmc5843.type = I2CTransTxRx;
+ ppzuavimu_hmc5843.len_r = 6;
+ ppzuavimu_hmc5843.len_w = 1;
+ ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
+ i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+#if PERIODIC_FREQUENCY > 60
+ });
+#endif
+ //RunOnceEvery(10,ppzuavimu_module_downlink_raw());
+}
+
+void ppzuavimu_module_downlink_raw( void )
+{
+ DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
+}
+
+void ppzuavimu_module_event( void )
+{
+ int32_t x, y, z;
+
+ // If the itg3200 I2C transaction has succeeded: convert the data
+ if (ppzuavimu_itg3200.status == I2CTransSuccess)
+ {
+#define ITG_STA_DAT_OFFSET 3
+ x = (int16_t) ((ppzuavimu_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[1+ITG_STA_DAT_OFFSET]);
+ y = (int16_t) ((ppzuavimu_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[3+ITG_STA_DAT_OFFSET]);
+ z = (int16_t) ((ppzuavimu_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[5+ITG_STA_DAT_OFFSET]);
+
+ // Is this is new data
+ if (ppzuavimu_itg3200.buf[0] & 0x01)
+ {
+ //LED_ON(3);
+ gyr_valid = TRUE;
+ //LED_OFF(3);
+ }
+ else
+ {
+ }
+
+ // Signs depend on the way sensors are soldered on the board: so they are hardcoded
+#ifdef ASPIRIN_IMU
+ RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
+#else // PPZIMU
+ RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
+#endif
+
+ ppzuavimu_itg3200.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
+ }
+
+ // If the adxl345 I2C transaction has succeeded: convert the data
+ if (ppzuavimu_adxl345.status == I2CTransSuccess)
+ {
+ x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
+ y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
+ z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
+
+#ifdef ASPIRIN_IMU
+ VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
+#else // PPZIMU
+ VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
+#endif
+
+ acc_valid = TRUE;
+ ppzuavimu_adxl345.status = I2CTransDone;
+ }
+
+ // If the hmc5843 I2C transaction has succeeded: convert the data
+ if (ppzuavimu_hmc5843.status == I2CTransSuccess)
+ {
+ x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
+ y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
+ z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
+
+#ifdef ASPIRIN_IMU
+ VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
+#else // PPZIMU
+ VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
+#endif
+
+ mag_valid = TRUE;
+ ppzuavimu_hmc5843.status = I2CTransDone;
+ }
+}
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h
new file mode 100644
index 00000000000..8ffe325d299
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -0,0 +1,57 @@
+/*
+ * 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 PPZUAVIMU_H
+#define PPZUAVIMU_H
+
+#include "std.h"
+#include "subsystems/imu.h"
+
+extern volatile bool_t gyr_valid;
+extern volatile bool_t acc_valid;
+extern volatile bool_t mag_valid;
+
+/* must be defined in order to be IMU code: declared in imu.h
+extern void imu_impl_init(void);
+extern void imu_periodic(void);
+*/
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
+ ppzuavimu_module_event(); \
+ if (gyr_valid) { \
+ gyr_valid = FALSE; \
+ _gyro_handler(); \
+ } \
+ if (acc_valid) { \
+ acc_valid = FALSE; \
+ _accel_handler(); \
+ } \
+ if (mag_valid) { \
+ mag_valid = FALSE; \
+ _mag_handler(); \
+ } \
+}
+
+/* Own Extra Functions */
+extern void ppzuavimu_module_event( void );
+extern void ppzuavimu_module_downlink_raw( void );
+
+
+#endif // PPZUAVIMU_H
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index adb9d0513ec..c8be89d68c8 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -27,16 +27,19 @@
*/
#include "ins_module.h"
+#include "ins_xsens.h"
#include
#include "generated/airframe.h"
+#include "sys_time.h"
#include "downlink.h"
#include "messages.h"
#ifdef USE_GPS_XSENS
#include "subsystems/gps.h"
+#include "math/pprz_geodetic_wgs84.h"
#include "math/pprz_geodetic_float.h"
#include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
#endif
@@ -68,6 +71,8 @@ INS_FORMAT ins_mz;
float ins_pitch_neutral;
float ins_roll_neutral;
+volatile uint8_t new_ins_attitude;
+
//////////////////////////////////////////////////////////////////////////////////////////
//
// XSens Specific
@@ -161,11 +166,25 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
#define GOT_DATA 5
#define GOT_CHECKSUM 6
+// FIXME Debugging Only
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+
uint8_t xsens_errorcode;
uint8_t xsens_msg_status;
uint16_t xsens_time_stamp;
uint16_t xsens_output_mode;
uint32_t xsens_output_settings;
+float xsens_declination = 0;
+float xsens_gps_arm_x = 0;
+float xsens_gps_arm_y = 0;
+float xsens_gps_arm_z = 0;
+
int8_t xsens_hour;
int8_t xsens_min;
@@ -174,11 +193,6 @@ int32_t xsens_nanosec;
int16_t xsens_year;
int8_t xsens_month;
int8_t xsens_day;
-float xsens_lat;
-float xsens_lon;
-
-int8_t xsens_gps_nr_channels = 16;
-
static uint8_t xsens_id;
static uint8_t xsens_status;
@@ -190,9 +204,12 @@ uint8_t send_ck;
struct LlaCoor_f lla_f;
struct UtmCoor_f utm_f;
+volatile int xsens_configured = 0;
+
void ins_init( void ) {
xsens_status = UNINIT;
+ xsens_configured = 20;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -201,33 +218,105 @@ void ins_init( void ) {
xsens_time_stamp = 0;
xsens_output_mode = XSENS_OUTPUT_MODE;
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
- /* send mode and settings to MT */
- XSENS_GoToConfig();
- XSENS_SetOutputMode(xsens_output_mode);
- XSENS_SetOutputSettings(xsens_output_settings);
- //XSENS_GoToMeasurment();
+
+ gps.nb_channels = 0;
}
void ins_periodic_task( void ) {
+ if (xsens_configured > 0)
+ {
+ switch (xsens_configured)
+ {
+ case 20:
+ /* send mode and settings to MT */
+ XSENS_GoToConfig();
+ XSENS_SetOutputMode(xsens_output_mode);
+ XSENS_SetOutputSettings(xsens_output_settings);
+ break;
+ case 18:
+ // Give pulses on SyncOut
+ XSENS_SetSyncOutSettings(0,0x0002);
+ break;
+ case 17:
+ // 1 pulse every 100 samples
+ XSENS_SetSyncOutSettings(1,100);
+ break;
+ case 2:
+ XSENS_ReqLeverArmGps();
+ break;
+
+ case 6:
+ XSENS_ReqMagneticDeclination();
+ break;
+
+ case 13:
+ #ifdef AHRS_H_X
+ #warning Sending XSens Magnetic Declination
+ xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
+ XSENS_SetMagneticDeclination(xsens_declination);
+ #endif
+ break;
+ case 12:
+ #ifdef GPS_IMU_LEVER_ARM_X
+ #warning Sending XSens GPS Arm
+ XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
+ #endif
+ break;
+ case 10:
+ {
+ uint8_t baud = 1;
+ XSENS_SetBaudrate(baud);
+ }
+ break;
+
+ case 1:
+ XSENS_GoToMeasurment();
+ break;
+ }
+ xsens_configured--;
+ return;
+ }
+
RunOnceEvery(100,XSENS_ReqGPSStatus());
}
#include "estimator.h"
void handle_ins_msg( void) {
- EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
+
+
+ // Send to Estimator (Control)
+ EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
EstimatorSetRate(ins_p,ins_q);
+
+ // Position
+ float gps_east = gps.utm_pos.east / 100.;
+ float gps_north = gps.utm_pos.north / 100.;
+ gps_east -= nav_utm_east0;
+ gps_north -= nav_utm_north0;
+ EstimatorSetPosXY(gps_east, gps_north);
+
+ // Altitude and vertical speed
+ float hmsl = gps.hmsl;
+ hmsl /= 1000.0f;
+ EstimatorSetAlt(hmsl);
+
+ // Horizontal speed
+ float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D)
- gps.gspeed = 0;
+ {
+ fspeed = 0;
+ }
+ float fclimb = -ins_vz;
+ float fcourse = atan2f((float)ins_vy, (float)ins_vx);
+ EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
- gps.course = (atan2f((float)ins_vx, (float)ins_vy))*1e7;
- gps.ned_vel.z = (int16_t)(ins_vz * 100);
- gps.gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
- EstimatorSetAtt(ins_phi, ((float)gps.course / 1e7), ins_theta);
- // EstimatorSetAlt(ins_z);
- estimator_update_state_gps();
- // reset_gps_watchdog();
+ // Now also finish filling the gps struct for telemetry purposes
+ gps.gspeed = fspeed * 100.;
+ gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
+ gps.course = fcourse * 1e7;
+
}
void parse_ins_msg( void ) {
@@ -238,21 +327,41 @@ void parse_ins_msg( void ) {
else if (xsens_id == XSENS_ReqOutputSettings_ID) {
xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
}
+ else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
+ xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
+
+ DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+ }
+ else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
+ xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
+ xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
+ xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
+
+ DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+ }
else if (xsens_id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
}
#ifdef USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
- xsens_gps_nr_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
- gps.num_sv = xsens_gps_nr_channels;
+ gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
+ gps.num_sv = 0;
+
+ gps.last_fix_time = cpu_time_sec;
+
uint8_t i;
- for(i = 0; i < Min(xsens_gps_nr_channels, xsens_gps_nr_channels); i++) {
+ // Do not write outside buffer
+ for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
- if (ch > xsens_gps_nr_channels) continue;
+ if (ch > gps.nb_channels) continue;
gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
+ if (gps.svinfos[ch].flags > 0)
+ {
+ gps.num_sv++;
+ }
}
}
#endif
@@ -267,33 +376,49 @@ void parse_ins_msg( void ) {
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
+#ifdef GPS_LED
+ LED_TOGGLE(GPS_LED);
+#endif
+ gps.last_fix_time = cpu_time_sec;
gps.week = 0; // FIXME
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
+ gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
+
+
/* Set the real UTM zone */
gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
- lla_f.lat = (float) gps.lla_pos.lat * 1e7;
- lla_f.lon = (float) gps.lla_pos.lat * 1e7;
+ lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+ lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
+ gps.utm_pos.alt = gps.lla_pos.alt;
+
ins_x = utm_f.east;
ins_y = utm_f.north;
-
- gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 100;
- ins_z = -(INS_FORMAT)gps.hmsl / 1000.;
- ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
- ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
- ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
- gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
+ // Altitude: Xsens LLH gives ellipsoid height
+ ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
+
+ // Compute geoid (MSL) height
+ float hmsl;
+ WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
+ gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
+
+ ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
+ ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
+ ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
+ gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
+ gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
+ gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
- gps.pdop = 5;
+ gps.pdop = 5; // FIXME Not output by XSens
#endif
offset += XSENS_DATA_RAWGPS_LENGTH;
}
@@ -348,6 +473,7 @@ void parse_ins_msg( void ) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
offset += XSENS_DATA_Matrix_LENGTH;
}
+ new_ins_attitude = 1;
}
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
uint8_t l = 0;
@@ -361,6 +487,8 @@ void parse_ins_msg( void ) {
}
if (XSENS_MASK_Position(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
+ gps.last_fix_time = cpu_time_sec;
+
lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index 96248cef20b..790e65bf5a5 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -31,9 +31,6 @@
#include "std.h"
-extern float ins_pitch_neutral;
-extern float ins_roll_neutral;
-
extern int8_t xsens_hour;
extern int8_t xsens_min;
extern int8_t xsens_sec;
@@ -41,11 +38,8 @@ extern int32_t xsens_nanosec;
extern int16_t xsens_year;
extern int8_t xsens_month;
extern int8_t xsens_day;
-extern float xsens_lat;
-extern float xsens_lon;
extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp;
-
#endif
diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.c b/sw/airborne/modules/meteo/humid_sht_i2c.c
index 8fdccf61e45..3360eaa1a2c 100644
--- a/sw/airborne/modules/meteo/humid_sht_i2c.c
+++ b/sw/airborne/modules/meteo/humid_sht_i2c.c
@@ -35,6 +35,10 @@
#include "messages.h"
#include "downlink.h"
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
#ifndef SHT_I2C_DEV
#define SHT_I2C_DEV i2c0
#endif
diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c
index 1f56ce70f7b..736b3782c2b 100644
--- a/sw/airborne/modules/sensors/baro_ets.c
+++ b/sw/airborne/modules/sensors/baro_ets.c
@@ -98,7 +98,7 @@ void baro_ets_read_periodic( void ) {
I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2);
#else // SITL
baro_ets_adc = 0;
- baro_ets_altitude = gps.hms / 1000.0;
+ baro_ets_altitude = gps.hmsl / 1000.0;
baro_ets_valid = TRUE;
EstimatorSetAlt(baro_ets_altitude);
#endif
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
new file mode 100644
index 00000000000..cc3965c14de
--- /dev/null
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -0,0 +1,55 @@
+/*
+ * 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 "estimator.h"
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include
+
+#include "../../peripherals/hmc5843.h"
+
+
+int32_t mag_x, mag_y, mag_z;
+bool_t mag_valid;
+
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+void hmc5843_module_init( void ) {
+ hmc5843_init();
+}
+
+void hmc5843_module_periodic ( void )
+{
+ hmc5843_periodic();
+ mag_x = hmc5843.data.value[0];
+ mag_y = hmc5843.data.value[1];
+ mag_z = hmc5843.data.value[2];
+ RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+}
+
+void hmc5843_module_event( void )
+{
+ hmc5843_idle_task();
+}
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h
new file mode 100644
index 00000000000..d21dbb50379
--- /dev/null
+++ b/sw/airborne/modules/sensors/mag_hmc5843.h
@@ -0,0 +1,33 @@
+/*
+ * 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 HMC5843__H
+#define HMC5843__H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+extern int32_t mag_x, mag_y, mag_z;
+
+extern void hmc5843_module_init( void );
+extern void hmc5843_module_periodic( void );
+extern void hmc5843_module_event( void );
+
+#endif // HMC5843__H
diff --git a/sw/airborne/peripherals/adxl345.h b/sw/airborne/peripherals/adxl345.h
index de6fa023051..5e0b5559eff 100644
--- a/sw/airborne/peripherals/adxl345.h
+++ b/sw/airborne/peripherals/adxl345.h
@@ -1,6 +1,8 @@
#ifndef ADXL345_H
#define ADXL345_H
+#define ADXL345_ADDR 0xA6
+#define ADXL345_ADDR_ALT 0x3A
#define ADXL345_REG_BW_RATE 0x2C
#define ADXL345_REG_POWER_CTL 0x2D
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index f02c1483121..188b3d18b07 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -1,4 +1,6 @@
#include "peripherals/hmc5843.h"
+#include "mcu_periph/i2c.h"
+#include "led.h"
#define HMC5843_TIMEOUT 100
@@ -7,12 +9,18 @@
struct Hmc5843 hmc5843;
void exti9_5_irq_handler(void);
+#ifndef HMC5843_I2C_DEVICE
+#define HMC5843_I2C_DEVICE i2c2
+#endif
+
void hmc5843_init(void)
{
- hmc5843.i2c_trans.status = I2CTransSuccess;
- hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+ hmc5843.i2c_trans.status = I2CTransSuccess;
+ hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+#ifndef HMC5843_NO_IRQ
hmc5843_arch_init();
+#endif
}
// blocking, only intended to be called for initialization
@@ -22,62 +30,101 @@ static void send_config(void)
hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz
hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
hmc5843.i2c_trans.len_w = 2;
- i2c_submit(&i2c2,&hmc5843.i2c_trans);
+ i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending);
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
hmc5843.i2c_trans.buf[1] = 0x01<<5;
hmc5843.i2c_trans.len_w = 2;
- i2c_submit(&i2c2,&hmc5843.i2c_trans);
+ i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending);
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode
hmc5843.i2c_trans.buf[1] = 0x00;
hmc5843.i2c_trans.len_w = 2;
- i2c_submit(&i2c2,&hmc5843.i2c_trans);
+ i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending);
}
+#ifdef HMC5843_NO_IRQ
+volatile uint8_t fake_mag_eoc = 0;
+static uint8_t mag_eoc(void)
+{
+ return fake_mag_eoc;
+}
+#endif
+
void hmc5843_idle_task(void)
{
- if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
- if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
- hmc5843.ready_for_read = FALSE;
- hmc5843.i2c_trans.type = I2CTransRx;
- hmc5843.i2c_trans.len_r = 7;
- i2c_submit(&i2c2, &hmc5843.i2c_trans);
- hmc5843.reading = TRUE;
- }
+ if (hmc5843.i2c_trans.status == I2CTransFailed) {
+ hmc5843.sent_tx = 0;
+ hmc5843.sent_rx = 0;
+ }
+
+ if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return;
+
+ if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) {
+ if (HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)) {
+ hmc5843.i2c_trans.type = I2CTransTx;
+ hmc5843.i2c_trans.len_w = 1;
+ hmc5843.i2c_trans.buf[0] = 0x3;
+ i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+ hmc5843.sent_tx = 1;
+ return;
}
+ }
+
+ if (hmc5843.sent_tx) {
+ hmc5843.i2c_trans.type = I2CTransRx;
+ hmc5843.i2c_trans.len_r = 6;
+ hmc5843.i2c_trans.len_w = 1;
+ hmc5843.i2c_trans.buf[0] = 0x3;
+ i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+ hmc5843.sent_rx = 1;
+ hmc5843.sent_tx = 0;
+ return;
+ }
- if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
- hmc5843.timeout = 0;
- hmc5843.data_available = TRUE;
- hmc5843.reading = FALSE;
- memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
- for (int i = 0; i < 3; i++) {
- hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
- }
+ if (hmc5843.sent_rx && hmc5843.i2c_trans.status == I2CTransSuccess) {
+ hmc5843.sent_rx = 0;
+ hmc5843.sent_tx = 0;
+ hmc5843.timeout = 0;
+ hmc5843.data_available = TRUE;
+ memcpy(hmc5843.data.buf, (const void*) hmc5843.i2c_trans.buf, 6);
+ for (int i = 0; i < 3; i++) {
+ hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
}
+ }
}
void hmc5843_periodic(void)
{
- if (!hmc5843.initialized) {
- send_config();
- hmc5843.initialized = TRUE;
- } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
+ if (!hmc5843.initialized) {
+ send_config();
+ hmc5843.initialized = TRUE;
+ } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)){
#ifdef USE_HMC59843_ARCH_RESET
- hmc5843_arch_reset();
+ hmc5843_arch_reset();
#endif
- hmc5843.i2c_trans.type = I2CTransRx;
- hmc5843.i2c_trans.len_r = 7;
- i2c_submit(&i2c2, &hmc5843.i2c_trans);
- hmc5843.reading = TRUE;
- hmc5843.ready_for_read = FALSE;
- hmc5843.timeout = 0;
- }
+ hmc5843.i2c_trans.type = I2CTransTx;
+ hmc5843.i2c_trans.len_w = 1;
+ hmc5843.i2c_trans.buf[0] = 0x3;
+ i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+ while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
+
+ hmc5843.i2c_trans.type = I2CTransRx;
+ hmc5843.i2c_trans.len_r = 6;
+ i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+ while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
+ hmc5843.timeout = 0;
+ }
+
+#ifdef HMC5843_NO_IRQ
+ // < 50Hz
+ fake_mag_eoc = 1;
+#endif
+
}
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index ed31ebeed33..a198ddab116 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -27,25 +27,27 @@
#include "std.h"
#include "mcu_periph/i2c.h"
-#include "peripherals/hmc5843_arch.h"
-
struct Hmc5843 {
struct i2c_transaction i2c_trans;
+ uint32_t timeout;
+ uint8_t sent_tx;
+ uint8_t sent_rx;
+ uint8_t initialized;
+ uint8_t data_available;
union {
uint8_t buf[7];
int16_t value[3];
} data;
- uint8_t initialized;
- uint8_t reading;
- volatile uint8_t ready_for_read;
- uint8_t data_available;
- uint32_t timeout;
};
extern struct Hmc5843 hmc5843;
+#ifndef HMC5843_NO_IRQ
+#include "peripherals/hmc5843_arch.h"
+
extern void hmc5843_arch_init( void );
extern void hmc5843_arch_reset( void );
+#endif
extern void hmc5843_init(void);
extern void hmc5843_periodic(void);
diff --git a/sw/airborne/print.h b/sw/airborne/print.h
index 8f51c6286df..c62bee3872f 100644
--- a/sw/airborne/print.h
+++ b/sw/airborne/print.h
@@ -59,6 +59,8 @@
_PrintHex16(out_fun, low32); \
}
+#ifdef USE_UART0
+
#define Uart0PrintHex(c) _PrintHex(uart0_transmit, c)
#define Uart0PrintHex16(c) _PrintHex16(uart0_transmit, c)
#define Uart0PrintHex32(c) _PrintHex32(uart0_transmit, c)
@@ -69,8 +71,13 @@
#define UART0PrintHex32 Uart0PrintHex32
#define UART0PrintString Uart0PrintString
+#endif /* USE_UART0 */
+
+#ifdef USE_UART1
+
#define Uart1PrintHex(c) _PrintHex(uart1_transmit, c)
#define Uart1PrintHex16(c) _PrintHex16(uart1_transmit, c)
+#define Uart1PrintHex32(c) _PrintHex32(uart1_transmit, c)
#define Uart1PrintString(s) _PrintString(uart1_transmit, s)
#define UART1PrintHex Uart1PrintHex
@@ -78,6 +85,50 @@
#define UART1PrintHex32 Uart1PrintHex32
#define UART1PrintString Uart1PrintString
+#endif /* USE_UART1 */
+
+#ifdef USE_UART2
+
+#define Uart2PrintHex(c) _PrintHex(uart2_transmit, c)
+#define Uart2PrintHex16(c) _PrintHex16(uart2_transmit, c)
+#define Uart2PrintHex32(c) _PrintHex32(uart2_transmit, c)
+#define Uart2PrintString(s) _PrintString(uart2_transmit, s)
+
+#define UART2PrintHex Uart2PrintHex
+#define UART2PrintHex16 Uart2PrintHex16
+#define UART2PrintHex32 Uart2PrintHex32
+#define UART2PrintString Uart2PrintString
+
+#endif /* USE_UART2 */
+
+#ifdef USE_UART3
+
+#define Uart3PrintHex(c) _PrintHex(uart3_transmit, c)
+#define Uart3PrintHex16(c) _PrintHex16(uart3_transmit, c)
+#define Uart3PrintHex32(c) _PrintHex32(uart3_transmit, c)
+#define Uart3PrintString(s) _PrintString(uart3_transmit, s)
+
+#define UART3PrintHex Uart3PrintHex
+#define UART3PrintHex16 Uart3PrintHex16
+#define UART3PrintHex32 Uart3PrintHex32
+#define UART3PrintString Uart3PrintString
+
+#endif /* USE_UART3 */
+
+#ifdef USE_UART5
+
+#define Uart5PrintHex(c) _PrintHex(uart5_transmit, c)
+#define Uart5PrintHex16(c) _PrintHex16(uart5_transmit, c)
+#define Uart5PrintHex32(c) _PrintHex32(uart5_transmit, c)
+#define Uart5PrintString(s) _PrintString(uart5_transmit, s)
+
+#define UART5PrintHex Uart5PrintHex
+#define UART5PrintHex16 Uart5PrintHex16
+#define UART5PrintHex32 Uart5PrintHex32
+#define UART5PrintString Uart5PrintString
+
+#endif /* USE_UART5 */
+
#define UsbSPrintHex(c) _PrintHex(VCOM_putchar, c)
#define UsbSPrintHex16(c) _PrintHex16(VCOM_putchar, c)
#define UsbSPrintString(s) _PrintString(VCOM_putchar, s)
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 04e30d69db6..4438c4d0637 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -73,7 +73,8 @@ float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
#ifdef USE_MAGNETOMETER
-float MAG_Heading;
+float MAG_Heading_X = 1;
+float MAG_Heading_Y = 0;
#endif
static inline void compute_body_orientation_and_rates(void);
@@ -113,7 +114,7 @@ void ahrs_update_fw_estimator( void )
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
estimator_p = Omega_Vector[0];
-
+/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
@@ -128,7 +129,7 @@ void ahrs_update_fw_estimator( void )
&(DCM_Matrix[2][2])
));
-
+*/
}
@@ -213,6 +214,13 @@ void ahrs_update_accel(void)
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
+ // DCM filter uses g-force as positive
+ // accelerometer measures [0 0 -g] in a static case
+ accel_float.x = -accel_float.x;
+ accel_float.y = -accel_float.y;
+ accel_float.z = -accel_float.z;
+
+
#ifdef USE_GPS
if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration.
accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
@@ -223,9 +231,58 @@ void ahrs_update_accel(void)
Drift_correction();
}
+
void ahrs_update_mag(void)
{
- //TODO
+#ifdef USE_MAGNETOMETER
+#warning MAGNETOMETER FEEDBACK NOT TESTED YET
+
+ float cos_roll;
+ float sin_roll;
+ float cos_pitch;
+ float sin_pitch;
+
+ cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi);
+ sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi);
+ cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta);
+ sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta);
+
+
+ // Pitch&Roll Compensation:
+ MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch;
+ MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll;
+
+/*
+ *
+ // Magnetic Heading
+ Heading = atan2(-Head_Y,Head_X);
+
+ // Declination correction (if supplied)
+ if( declination != 0.0 )
+ {
+ Heading = Heading + declination;
+ if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
+ Heading -= (2.0 * M_PI);
+ else if (Heading < -M_PI)
+ Heading += (2.0 * M_PI);
+ }
+
+ // Optimization for external DCM use. Calculate normalized components
+ Heading_X = cos(Heading);
+ Heading_Y = sin(Heading);
+*/
+
+ struct FloatVect3 ltp_mag;
+
+ ltp_mag.x = MAG_Heading_X;
+ ltp_mag.y = MAG_Heading_Y;
+
+ // Downlink
+ RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, <p_mag.x, <p_mag.y, <p_mag.z));
+
+ // Magnetic Heading
+ // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
+#endif
}
void Normalize(void)
@@ -359,9 +416,10 @@ void Drift_correction(void)
#ifdef USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
- float mag_heading_x = cos(MAG_Heading);
- float mag_heading_y = sin(MAG_Heading);
- errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
+// float mag_heading_x = cos(MAG_Heading);
+// float mag_heading_y = sin(MAG_Heading);
+ // 2D dot product
+ errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index ea54f0c1d45..f731cf5a71d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -53,7 +53,9 @@ void ahrs_update_fw_estimator(void);
#define GRAVITY 9.81
+#ifndef OUTPUTMODE
#define OUTPUTMODE 1
+#endif
// Mode 0 = DCM integration without Ki gyro bias
// Mode 1 = DCM integration with Kp and Ki
// Mode 2 = direct accelerometer -> euler
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
index 03374c81498..69ac9c28c2a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
@@ -1,6 +1,8 @@
#ifndef AHRS_FLOAT_UTILS_H
#define AHRS_FLOAT_UTILS_H
+#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
+
static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
/* get phi and theta from accelerometer */
struct FloatVect3 accelf;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
index a74f631cb38..f398bdd891d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
@@ -100,7 +100,7 @@ void ahrs_init(void) {
void ahrs_align(void) {
-
+
/* Compute an initial orientation using euler angles */
ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
@@ -215,12 +215,12 @@ void ahrs_update_mag(void) {
static inline void ahrs_update_mag_full(void) {
- const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
- MAG_BFP_OF_REAL(AHRS_H_Y),
+ const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
+ MAG_BFP_OF_REAL(AHRS_H_Y),
MAG_BFP_OF_REAL(AHRS_H_Z)};
struct Int32Vect3 expected_imu;
INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
-
+
struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
@@ -228,12 +228,12 @@ static inline void ahrs_update_mag_full(void) {
ahrs_impl.rate_correction.q += residual.y/32/16;
ahrs_impl.rate_correction.r += residual.z/32/16;
-
+
ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
-
+
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
@@ -241,17 +241,17 @@ static inline void ahrs_update_mag_full(void) {
static inline void ahrs_update_mag_2d(void) {
- const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
+ const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y)};
struct Int32Vect3 measured_ltp;
INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
-
- struct Int32Vect3 residual_ltp =
+
+ struct Int32Vect3 residual_ltp =
{ 0,
0,
(measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
-
+
struct Int32Vect3 residual_imu;
INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
@@ -266,7 +266,7 @@ static inline void ahrs_update_mag_2d(void) {
ahrs_impl.rate_correction.p += residual_imu.x/16;
ahrs_impl.rate_correction.q += residual_imu.y/16;
ahrs_impl.rate_correction.r += residual_imu.z/16;
-
+
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// high_rez_bias = RATE_FRAC+28 = 40
@@ -300,12 +300,12 @@ __attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_fro
/* Compute ltp to imu rotation in euler angles and rotation matrice representation
from the quaternion representation */
__attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {
-
+
/* Compute LTP to IMU euler */
INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat);
/* Compute LTP to IMU rotation matrix */
INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat);
-
+
}
__attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
@@ -320,3 +320,27 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
}
+
+// TODO use ahrs result directly
+#include "estimator.h"
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+void ahrs_update_fw_estimator(void)
+{
+ struct FloatEulers att;
+ // export results to estimator
+ EULERS_FLOAT_OF_BFP(att,ahrs.ltp_to_imu_euler);
+
+ estimator_phi = att.phi;
+ estimator_theta = att.theta;
+ estimator_psi = att.psi;
+
+ //estimator_p = Omega_Vector[0];
+}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
index f1f833b656f..ab0a7d939dc 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
@@ -40,4 +40,12 @@ struct AhrsIntCmpl {
extern struct AhrsIntCmpl ahrs_impl;
+
+// TODO copy ahrs to state instead of estimator
+void ahrs_update_fw_estimator(void);
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+
+
+
#endif /* AHRS_INT_CMPL_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
index 84918fcc620..73450f57916 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
@@ -4,7 +4,7 @@
//#include "../../test/pprz_algebra_print.h"
#include "math/pprz_algebra_int.h"
-#include "generated/airframe.h"
+#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
// DISPLAY_INT32_VECT3("# accel", (*accel));
@@ -17,7 +17,7 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru
int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
const float ftheta = atan2f(-cphi_ax, -accel->z);
e->theta = ANGLE_BFP_OF_REAL(ftheta);
-
+
int32_t sphi;
PPRZ_ITRIG_SIN(sphi, e->phi);
int32_t stheta;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
new file mode 100644
index 00000000000..b548ea3b048
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
@@ -0,0 +1,10 @@
+#include "generated/airframe.h"
+
+#ifndef AHRS_H_X
+#ifdef AHRS_H_Y
+#error Either define both AHRS_H_X and AHRS_H_Y or none, but not half
+#endif
+#define AHRS_H_X 1
+#define AHRS_H_Y 0
+#endif
+
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index ace5586b3db..21a30c6bb1f 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -78,8 +78,7 @@ struct GpsState {
uint8_t nb_channels; ///< Number of scanned satellites
struct SVinfo svinfos[GPS_NB_CHANNELS];
- uint8_t lost_counter; /* updated at 4Hz */
- uint16_t last_msg_time;
+ uint16_t last_fix_time; ///< cpu time in sec at last valid fix
uint16_t reset; ///< hotstart, warmstart, coldstart
};
@@ -99,15 +98,11 @@ extern void gps_init(void);
extern void gps_impl_init(void);
-
-//TODO
-// this is only true for a 512Hz main loop
-// needs to work with different main loop frequencies
-static inline void gps_periodic( void ) {
- RunOnceEvery(128, gps.lost_counter++; );
-}
-
-#define GpsIsLost() (gps.lost_counter > 20) /* 4Hz -> 5s */
+/* mark GPS as lost when no valid 3D fix was received for GPS_TIMEOUT secs */
+#ifndef GPS_TIMEOUT
+#define GPS_TIMEOUT 5
+#endif
+#define GpsIsLost() (cpu_time_sec - gps.last_fix_time > GPS_TIMEOUT)
//TODO
diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h
index 7e20ed92351..7794ea9c248 100644
--- a/sw/airborne/subsystems/gps/gps_sim.h
+++ b/sw/airborne/subsystems/gps/gps_sim.h
@@ -14,8 +14,7 @@ extern void gps_impl_init(void);
#define GpsEvent(_sol_available_callback) { \
if (gps_available) { \
if (gps.fix == GPS_FIX_3D) { \
- gps.lost_counter = 0; \
- gps.last_msg_time = cpu_time_sec; \
+ gps.last_fix_time = cpu_time_sec; \
} \
_sol_available_callback(); \
gps_available = FALSE; \
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h
index c5284b2e64a..c14e3f8f946 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.h
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.h
@@ -14,7 +14,7 @@ extern void gps_impl_init();
#define GpsEvent(_sol_available_callback) { \
if (gps_available) { \
if (gps.fix == GPS_FIX_3D) \
- gps.lost_counter = 0; \
+ gps.last_fix_time = cpu_time_sec; \
_sol_available_callback(); \
gps_available = FALSE; \
} \
diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h
index c5d6964c92b..fea8398af0c 100644
--- a/sw/airborne/subsystems/gps/gps_skytraq.h
+++ b/sw/airborne/subsystems/gps/gps_skytraq.h
@@ -88,19 +88,19 @@ extern struct GpsSkytraq gps_skytraq;
#define GpsBuffer() GpsLink(ChAvailable())
-#define GpsEvent(_sol_available_callback) { \
- if (GpsBuffer()) { \
- ReadGpsBuffer(); \
- } \
- if (gps_skytraq.msg_available) { \
- gps_skytraq_read_message(); \
+#define GpsEvent(_sol_available_callback) { \
+ if (GpsBuffer()) { \
+ ReadGpsBuffer(); \
+ } \
+ if (gps_skytraq.msg_available) { \
+ gps_skytraq_read_message(); \
if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \
- if (gps.fix == GPS_FIX_3D) \
- gps.lost_counter = 0; \
- _sol_available_callback(); \
- } \
- gps_skytraq.msg_available = FALSE; \
- } \
+ if (gps.fix == GPS_FIX_3D) \
+ gps.last_fix_time = cpu_time_sec; \
+ _sol_available_callback(); \
+ } \
+ gps_skytraq.msg_available = FALSE; \
+ } \
}
#define ReadGpsBuffer() { \
diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 911ead5a351..6a8d3fd61cc 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -102,7 +102,7 @@ void gps_ubx_read_message(void) {
if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
#ifdef GPS_TIMESTAMP
/* get hardware clock ticks */
- gps.t0 = T0TC;
+ SysTimeTimerStart(gps.t0);
gps.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
#endif
@@ -135,8 +135,8 @@ void gps_ubx_read_message(void) {
#ifdef GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
- lla_f.lat = (float) gps.lla_pos.lat * 1e7;
- lla_f.lon = (float) gps.lla_pos.lat * 1e7;
+ lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+ lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
@@ -155,6 +155,8 @@ void gps_ubx_read_message(void) {
if (hem == UTM_HEM_SOUTH)
gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10;
+ gps.hmsl = gps.utm_pos.alt;
+ gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
#endif
}
@@ -164,7 +166,11 @@ void gps_ubx_read_message(void) {
gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
- gps.course = RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*100);
+ // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
+ // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
+ // solution: First to radians, and then scale to 1e-7 radians
+ // First x 10 for loosing less resolution, then to radians, then multiply x 10 again
+ gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10;
gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
}
else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
@@ -179,6 +185,11 @@ void gps_ubx_read_message(void) {
gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
}
}
+ else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
+ gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
+ gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
+ gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
+ }
}
}
@@ -288,11 +299,9 @@ void gps_ubx_parse( uint8_t c ) {
#define GPS_PORT_ID GPS_PORT_UART1
#endif
-#if GPS_LINK == UART0
-#define UBX_GPS_BAUD UART0_BAUD
-#elif GPS_LINK == UART1
-#define UBX_GPS_BAUD UART1_BAUD
-#endif
+#define __UBX_GPS_BAUD(_u) _u##_BAUD
+#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
+#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
/* Configure the GPS baud rate using the current uart baud rate. Busy
wait for the end of the transmit. Then, BEFORE waiting for the ACK,
diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h
index 321bd24810e..e1f85d4d410 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.h
+++ b/sw/airborne/subsystems/gps/gps_ubx.h
@@ -48,6 +48,9 @@ struct GpsUbx {
uint8_t send_ck_a, send_ck_b;
uint8_t error_cnt;
uint8_t error_last;
+
+ uint8_t status_flags;
+ uint8_t sol_flags;
};
extern struct GpsUbx gps_ubx;
@@ -83,8 +86,7 @@ extern bool_t gps_configuring;
if (gps_ubx.msg_class == UBX_NAV_ID && \
gps_ubx.msg_id == UBX_NAV_VELNED_ID) { \
if (gps.fix == GPS_FIX_3D) { \
- gps.lost_counter = 0; \
- gps.last_msg_time = cpu_time_sec; \
+ gps.last_fix_time = cpu_time_sec; \
} \
_sol_available_callback(); \
} \
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.c b/sw/airborne/subsystems/imu/imu_aspirin.c
index c8922066d28..86cb2a87640 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin.c
@@ -26,9 +26,7 @@ static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
void imu_impl_init(void) {
imu_aspirin.status = AspirinStatusUninit;
- imu_aspirin.gyro_available = FALSE;
imu_aspirin.gyro_available_blaaa = FALSE;
- imu_aspirin.mag_ready_for_read = FALSE;
imu_aspirin.mag_available = FALSE;
imu_aspirin.accel_available = FALSE;
@@ -37,6 +35,7 @@ void imu_impl_init(void) {
imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
imu_aspirin.i2c_trans_gyro.len_w = 1;
imu_aspirin.i2c_trans_gyro.len_r = 6;
+ imu_aspirin.i2c_trans_gyro.status = I2CTransFailed;
imu_aspirin_arch_init();
hmc5843_init();
@@ -51,10 +50,14 @@ void imu_periodic(void) {
configure_accel();
imu_aspirin_arch_int_enable();
imu_aspirin.status = AspirinStatusIdle;
- }
- else {
+ } else {
imu_aspirin.gyro_available_blaaa = TRUE;
imu_aspirin.time_since_last_reading++;
+ imu_aspirin.time_since_last_accel_reading++;
+ if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
+ configure_accel();
+ imu_aspirin.time_since_last_accel_reading=0;
+ }
}
}
@@ -78,9 +81,9 @@ static void configure_gyro(void) {
t.buf[0] = ITG3200_REG_PWR_MGM;
t.buf[1] = 0x01;
send_i2c_msg_with_retry(&t);
- /* enable interrupt on data ready, idle hight */
+ /* enable interrupt on data ready, idle high, latch until read any register */
t.buf[0] = ITG3200_REG_INT_CFG;
- t.buf[1] = (0x01 | 0x01<<7);
+ t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
send_i2c_msg_with_retry(&t);
}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index f937fb8fd54..f9ac1d4f0a4 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -63,19 +63,19 @@ struct ImuAspirin {
volatile enum AspirinStatus status;
struct i2c_transaction i2c_trans_gyro;
struct i2c_transaction i2c_trans_mag;
- uint8_t gyro_eoc;
- uint8_t gyro_available;
- uint8_t gyro_available_blaaa;
- uint8_t mag_available;
- volatile uint8_t mag_ready_for_read;
volatile uint8_t accel_available;
volatile uint8_t accel_tx_buf[7];
volatile uint8_t accel_rx_buf[7];
uint32_t time_since_last_reading;
+ uint32_t time_since_last_accel_reading;
+ uint8_t mag_available;
+ uint8_t reading_gyro;
+ uint8_t gyro_available_blaaa;
};
extern struct ImuAspirin imu_aspirin;
#define ASPIRIN_GYRO_TIMEOUT 3
+#define ASPIRIN_ACCEL_TIMEOUT 3
#include "peripherals/hmc5843.h"
#define foo_handler() {}
@@ -99,6 +99,7 @@ static inline void gyro_read_i2c(void)
{
imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
+ imu_aspirin.reading_gyro = 1;
}
static inline void gyro_copy_i2c(void)
@@ -117,45 +118,70 @@ static inline void accel_copy_spi(void)
VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
}
+static inline void imu_gyro_event(void (* _gyro_handler)(void))
+{
+
+}
+
static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
if (imu_aspirin.status == AspirinStatusUninit) return;
imu_aspirin_arch_int_disable();
- ImuMagEvent(_mag_handler);
+ if (imu_aspirin.accel_available) {
+ imu_aspirin.time_since_last_accel_reading = 0;
+ imu_aspirin.accel_available = FALSE;
+ accel_copy_spi();
+ _accel_handler();
+ }
+ imu_aspirin_arch_int_enable();
+
+ // Reset everything if we've been waiting too long
if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
- imu_aspirin.gyro_eoc = FALSE;
- imu_aspirin.status = AspirinStatusIdle;
i2c2_er_irq_handler();
gyro_read_i2c();
imu_aspirin.time_since_last_reading = 0;
+ return;
}
- if (imu_aspirin.status == AspirinStatusReadingGyro && imu_aspirin.gyro_eoc &&
- imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) {
- gyro_copy_i2c();
- imu_aspirin.status = AspirinStatusIdle;
- if (imu_aspirin.gyro_available_blaaa) {
- imu_aspirin.gyro_available_blaaa = FALSE;
- _gyro_handler();
+
+ // Try again later if transaction is in progress
+ if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning)
+ {
+ return;
+ }
+
+ ImuMagEvent(_mag_handler);
+
+ // Try back later if things are not idle
+ if ((i2c2.status != I2CIdle) || !i2c_idle(&i2c2)) {
+ return;
+ }
+
+ if (imu_aspirin.reading_gyro) {
+ if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
+ gyro_copy_i2c();
+ if (imu_aspirin.gyro_available_blaaa) {
+ imu_aspirin.gyro_available_blaaa = FALSE;
+ _gyro_handler();
+ }
}
+ imu_aspirin.reading_gyro = 0;
+ return;
}
- if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
+
+ // If we're not already waiting for read, and conversion is complete, schedule a read
+ if (!imu_aspirin.reading_gyro && imu_aspirin_eoc() && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
imu_aspirin.time_since_last_reading = 0;
}
- imu_aspirin.gyro_eoc = FALSE;
gyro_read_i2c();
+ return;
}
- if (imu_aspirin.accel_available) {
- imu_aspirin.accel_available = FALSE;
- accel_copy_spi();
- _accel_handler();
- }
- imu_aspirin_arch_int_enable();
+
}
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \
imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
- } while(0);
+} while(0);
#endif /* IMU_ASPIRIN_H */
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
index c506be572c1..219e7786032 100644
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ b/sw/airborne/subsystems/navigation/spiral.c
@@ -37,7 +37,7 @@ static float SRad;
static float IRad;
static float Alphalimit;
static float Segmente;
-static float CamAngle;
+// static float CamAngle;
static float ZPoint;
static float nav_radius_min;
diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.c b/sw/airborne/subsystems/radio_control/rc_datalink.c
index 662a6b5ccda..1d7a39ccb5e 100644
--- a/sw/airborne/subsystems/radio_control/rc_datalink.c
+++ b/sw/airborne/subsystems/radio_control/rc_datalink.c
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "radio_control/rc_datalink.h"
+#include "subsystems/radio_control/rc_datalink.h"
#include "subsystems/radio_control.h"
int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
@@ -33,11 +33,11 @@ void radio_control_impl_init(void) {
}
-void parse_rc_datalink( uint8_t throttle_mode,
+void parse_rc_3ch_datalink( uint8_t throttle_mode,
int8_t roll,
int8_t pitch)
{
- uint8_t throttle = throttle_mode & 0xFC;
+ uint8_t throttle = ((throttle_mode & 0xFC)>>2)*(128/64);
uint8_t mode = throttle_mode & 0x03;
rc_dl_values[RADIO_ROLL] = roll;
@@ -47,3 +47,20 @@ void parse_rc_datalink( uint8_t throttle_mode,
rc_dl_frame_available = TRUE;
}
+
+void parse_rc_4ch_datalink(
+ uint8_t mode,
+ uint8_t throttle,
+ int8_t roll,
+ int8_t pitch,
+ int8_t yaw)
+{
+ rc_dl_values[RADIO_MODE] = (int8_t)mode;
+ rc_dl_values[RADIO_THROTTLE] = (int8_t)throttle;
+ rc_dl_values[RADIO_ROLL] = roll;
+ rc_dl_values[RADIO_PITCH] = pitch;
+ rc_dl_values[RADIO_YAW] = yaw;
+
+ rc_dl_frame_available = TRUE;
+}
+
diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.h b/sw/airborne/subsystems/radio_control/rc_datalink.h
index d10170febbe..ca72750480d 100644
--- a/sw/airborne/subsystems/radio_control/rc_datalink.h
+++ b/sw/airborne/subsystems/radio_control/rc_datalink.h
@@ -43,13 +43,24 @@ extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
extern volatile bool_t rc_dl_frame_available;
/**
- * Decode datalink message to get rc values
+ * Decode datalink message to get rc values with RC_3CH message
+ * Mode and throttle are merge in the same byte
*/
-extern void parse_rc_datalink(
+extern void parse_rc_3ch_datalink(
uint8_t throttle_mode,
int8_t roll,
int8_t pitch);
+/**
+ * Decode datalink message to get rc values with RC_4CH message
+ */
+extern void parse_rc_4ch_datalink(
+ uint8_t mode,
+ uint8_t throttle,
+ int8_t roll,
+ int8_t pitch,
+ int8_t yaw);
+
/**
* Macro that normalize rc_dl_values to radio values
*/
@@ -60,7 +71,7 @@ extern void parse_rc_datalink(
Bound(_out[RADIO_PITCH], MIN_PPRZ, MAX_PPRZ); \
_out[RADIO_YAW] = 0; \
Bound(_out[RADIO_YAW], MIN_PPRZ, MAX_PPRZ); \
- _out[RADIO_THROTTLE] = ((MAX_PPRZ/64) * _in[RADIO_THROTTLE]); \
+ _out[RADIO_THROTTLE] = ((MAX_PPRZ/128) * _in[RADIO_THROTTLE]); \
Bound(_out[RADIO_THROTTLE], 0, MAX_PPRZ); \
_out[RADIO_MODE] = MAX_PPRZ * (_in[RADIO_MODE] - 1); \
Bound(_out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ); \
diff --git a/sw/ground_segment/joystick/README b/sw/ground_segment/joystick/README
index 6c6fb0bde19..abc7bc77b5b 100644
--- a/sw/ground_segment/joystick/README
+++ b/sw/ground_segment/joystick/README
@@ -21,7 +21,9 @@
#
-Tools for controling an aircraft with a ground joystick
+Tools for controlling an aircraft with a ground joystick
+
+Also used as general purpose joystick to ivybus connector. Can generate arbitrary ivybus messages based on control input from buttons or axes. Supports limiting, exponential and scaling on axes.
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 2caae99c231..a4eeaa9573c 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -24,12 +24,32 @@
*
*)
+(* 1/26/2011 - Additional functionality added by jpeverill:
+ Joystick xml config files now loaded from PAPARAZZI_HOME/conf/joystick/
+ Exponential output setting (per channel)
+ Limit output setting (per channel)
+ Per channel trim, controlled through joystick buttons
+ Trim can also be saved into auxilliary file, based on aircraft, and loaded at runtime if it exists
+ File will be called ..trim and is saved in the conf/joystick directory as well
+ Division in channel mapping
+ Interactive keyboard trim control (primitive)
+*)
+
open Printf
+open Unix
+open Random
+
let (//) = Filename.concat
let conf_dir = Env.paparazzi_home // "conf"
let verbose = ref false (* Command line option *)
+(** global trim file name *)
+let trim_file_name = ref ""
+
+(** global joystick id *)
+let joystick_id = ref (Random.int 255)
+
(** Messages libraries *)
module DL = Pprz.Messages(struct let name = "datalink" end)
module G = Pprz.Messages(struct let name = "ground" end)
@@ -55,10 +75,11 @@ external stick_read : unit -> int * int * int array = "ml_stick_read"
(** Range for the input axis *)
let max_input = 127
let min_input = -127
+let trim_step = 0.2
(** Representation of an input value *)
type input =
- Axis of int * int (* (index, deadband) *)
+ Axis of int * int * float * float * float ref (* (index, deadband, limit, exponent, trim ) *)
| Button of int
(** Description of a message *)
@@ -66,17 +87,40 @@ type msg = {
msg_name : string;
msg_class : string;
fields : (string * Syntax.expression) list;
- on_event : Syntax.expression option
+ on_event : Syntax.expression option;
+ send_always : bool
}
-(** Represenation of an input device and of the messages to send *)
+(** Representation of a variable *)
+type var = {
+ mutable value : int;
+ var_event : (int * Syntax.expression) list;
+}
+
+(** Represenation of an input device, the messages to send and the variables *)
type actions = {
period_ms : int;
inputs : (string*input) list;
- messages : msg list
+ messages : msg list;
+ variables : (string*var) list;
}
+(** adjust the trim on an axis given its name *)
+let trim_adjust = fun axis_name adjustment ->
+ None
+
+(** Get message class type *)
+let get_message_type = fun class_name ->
+ match class_name with
+ "datalink" -> "Message"
+ | "ground" -> "Message"
+ | "trim_plus" -> "Trim"
+ | "trim_minus" -> "Trim"
+ | "trim_save" -> "Trim"
+ | _ -> failwith class_name
+
(** Get a message description from its name (and class name) *)
+(** class_names with entries above as "Message" should be listed here *)
let get_message = fun class_name msg_name ->
match class_name with
"datalink" -> snd (DL.message_of_name msg_name)
@@ -152,19 +196,21 @@ let eval_settings_and_blocks = fun field_descr expr ->
(** Parse an XML list of input channels *)
let parse_input = fun input ->
- List.map
- (fun x ->
- let name = Xml.attrib x "name"
- and index = ExtXml.int_attrib x "index" in
- let value =
- match Xml.tag x with
- "axis" ->
- let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
- Axis (index, deadband)
- | "button" -> Button index
- | _ -> failwith "parse_input: unexepcted tag" in
- (name, value))
- (Xml.children input)
+ List.map (fun x ->
+ let name = Xml.attrib x "name"
+ and index = ExtXml.int_attrib x "index" in
+ let value =
+ match Xml.tag x with
+ "axis" ->
+ let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
+ let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0.0 in
+ let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
+ let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
+ Axis (index, deadband, limit, exponent, ref trim)
+ | "button" -> Button index
+ | _ -> failwith "parse_input: unexepcted tag" in
+ (name, value))
+ (Xml.children input)
(** Parse a 'à la C' expression *)
let parse_value = fun s ->
@@ -181,11 +227,16 @@ let parse_msg_field = fun msg_descr field ->
(** Parse a complete message and build its representation *)
let parse_msg = fun msg ->
let msg_name = Xml.attrib msg "name"
- and msg_class = Xml.attrib msg "class" in
+ and msg_class = Xml.attrib msg "class"
+ and send_always = (try (Xml.attrib msg "send_always") = "true" with _ -> false) in
- let msg_descr = get_message msg_class msg_name in
-
- let fields = List.map (parse_msg_field msg_descr) (Xml.children msg) in
+ let fields =
+ match get_message_type msg_class with
+ "Message" ->
+ let msg_descr = get_message msg_class msg_name in
+ List.map (parse_msg_field msg_descr) (Xml.children msg)
+ | "Trim" -> []
+ | _ -> failwith ("Unknown message class type") in
let on_event =
try Some (parse_value (Xml.attrib msg "on_event")) with _ -> None in
@@ -193,59 +244,167 @@ let parse_msg = fun msg ->
{ msg_name = msg_name;
msg_class = msg_class;
fields = fields;
- on_event = on_event
+ on_event = on_event;
+ send_always = send_always
}
-(** Parse the complete (input and messages) XML desxription *)
-let parse_descr = fun xml_file ->
+(** Parse an XML list of variables and set function *)
+let parse_variables = fun variables ->
+ let l = ref [] in
+ List.iter (fun x ->
+ match Xml.tag x with
+ "var" ->
+ let name = Xml.attrib x "name"
+ and default = ExtXml.int_attrib x "default" in
+ if List.mem_assoc name !l then failwith (sprintf "Variable %s already declared" name);
+ (* filter all "set" node for this variable *)
+ let set = List.filter (fun vs ->
+ (ExtXml.tag_is vs "set") &&
+ (compare (ExtXml.attrib_or_default vs "var" "") name) = 0)
+ (Xml.children variables) in
+ let var_event = List.map (fun s ->
+ let value = ExtXml.int_attrib s "value"
+ and on_event = parse_value (Xml.attrib s "on_event") in
+ (value, on_event)
+ ) set in
+ l := (name, { value = default; var_event = var_event; }) :: !l;
+ ()
+ | _ -> ()
+ ) (Xml.children variables);
+ !l
+
+
+(** Verbose List.assoc *)
+let my_assoc = fun x l ->
+ try List.assoc x l with Not_found ->
+ failwith (sprintf "my_assoc: %s not found" x)
+
+let first_of_two (x,_) = x
+
+let second_of_two (_,x) = x
+
+(** set a trim value given an inputs array and a trim_values tuple *)
+let trim_set = fun inputs value ->
+ let input = my_assoc (first_of_two value) inputs in
+ match input with
+ Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
+ | Button i -> failwith "No trim for buttons"
+
+
+(** Input the trim file if it exists *)
+let parse_trim_file = fun trim_file_name inputs ->
+ if Sys.file_exists trim_file_name then begin
+ let trim = Xml.parse_file trim_file_name in
+ let trim_values = List.map
+ (fun x ->
+ let axis = ExtXml.attrib x "axis"
+ and trimval = ExtXml.float_attrib x "value" in
+ (axis, trimval))
+ (Xml.children trim) in
+ List.iter (trim_set inputs) trim_values;
+ end
+
+(** Parse the complete (input and messages) XML desxription
+ Also parses the trim xml file if it exists *)
+let parse_descr = fun xml_file trim_file ->
let xml = Xml.parse_file xml_file in
let inputs = parse_input (ExtXml.child xml "input")
- and messages_xml = ExtXml.child xml "messages" in
+ and messages_xml = ExtXml.child xml "messages"
+ and variables = try parse_variables (ExtXml.child xml "variables") with _ -> [] in
- let period_ms =truncate (1000.*.ExtXml.float_attrib messages_xml "period")
+ let period_ms = int_of_float (1000.*.ExtXml.float_attrib messages_xml "period")
and messages = List.map parse_msg (Xml.children messages_xml) in
- { period_ms = period_ms; inputs = inputs; messages = messages }
+ (** check for trim file *)
+ parse_trim_file trim_file inputs;
-(** Verbose List.assco *)
-let my_assoc = fun x l ->
- try List.assoc x l with Not_found ->
- failwith (sprintf "my_assoc: %s not found" x)
+ { period_ms = period_ms; inputs = inputs; messages = messages; variables = variables }
+(** apply deadband - applied first *)
let apply_deadband = fun x min ->
if abs x < min then 0 else x
+(** apply limit - applied third *)
+let apply_limit = fun x limit ->
+ limit *. x
+
+(** apply exponent - applied second *)
+let apply_exponent = fun x expon ->
+ let pow_value = (float_of_int x) ** 3. /. (float_of_int max_input) ** 2. in
+ ( (float_of_int x) *. (1. -. expon)) +. (pow_value *. expon)
+
+(** apply trim - applied fourth *)
+let apply_trim = fun x trim ->
+ let x_new = (int_of_float (x +. trim)) in
+ if x_new > max_input then max_input else (if x_new < min_input then min_input else x_new)
+
(** Access to an input value, button or axis *)
let eval_input = fun buttons axis input ->
match input with
- Axis (i, deadband) -> apply_deadband axis.(i) deadband
+ Axis (i, deadband, limit, exponent, trim) -> (apply_trim (apply_limit (apply_exponent (apply_deadband axis.(i) deadband) exponent) limit) trim.contents)
| Button i -> (buttons lsr i) land 0x1
(** Scale a value in the given bounds *)
let scale = fun x min max ->
min + ((x - min_input) * (max - min)) / (max_input - min_input)
+(** Scale a value in the given bounds *)
+let bound = fun x min max ->
+ if x < min then min else (if x > max then max else x)
+
+(** Fit a given interval of value into [min_input; max_input] *)
+let fit = fun x min max min_input max_input ->
+ let v = min_input + ((x - min) * (max_input - min_input)) / (max - min) in
+ bound v min_input max_input
+
+(** Return a pprz RC mode
+ * mode > max -> 2
+ * mode < min -> 0
+ * else 1
+ *)
+let pprz_threshold = max_input / 2
+let pprz_mode = fun mode ->
+ if mode > pprz_threshold then 2
+ else if mode < -pprz_threshold then 0
+ else 1
+
(** Eval a function call (TO BE COMPLETED) *)
let eval_call = fun f args ->
match f, args with
- "-", [a1; a2] -> a1 - a2
+ "-", [a] -> - a
+ | "-", [a1; a2] -> a1 - a2
| "+", [a1; a2] -> a1 + a2
| "*", [a1; a2] -> a1 * a2
+ | "%", [a1; a2] -> a1 / a2
| "&&", [a1; a2] -> a1 land a2
+ | "||", [a1; a2] -> a1 lor a2
+ | "<", [a1; a2] -> if a1 < a2 then 1 else 0
+ | ">", [a1; a2] -> if a1 > a2 then 1 else 0
| "Scale", [x; min; max] -> scale (x) (min) (max)
+ | "Fit", [x; min; max; min_input; max_input] -> fit (x) (min) (max) (min_input) (max_input)
+ | "Bound", [x; min; max] -> bound (x) (min) (max)
+ | "PprzMode", [x] -> pprz_mode (x)
+ | "JoystickID", [] -> !joystick_id
| f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
(** Eval an expression *)
-let eval_expr = fun buttons axis inputs expr ->
+let eval_expr = fun buttons axis inputs variables expr ->
let rec eval = function
Syntax.Ident ident ->
- let input = my_assoc ident inputs in
- eval_input buttons axis input
+ (* try input first, then variables *)
+ let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
+ (true, _) -> eval_input buttons axis (List.assoc ident inputs)
+ | (false, true) ->
+ let v = List.assoc ident variables in
+ v.value
+ | (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
+ in
+ i
| Syntax.Int int -> int
| Syntax.Float float -> failwith "eval_expr: float"
| Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
- eval_call ident (List.map eval exprs)
+ eval_call ident (List.map eval exprs)
| Syntax.Index _ -> failwith "eval_expr: index"
| Syntax.Field _ -> failwith "eval_expr: Field"
| Syntax.Deref _ -> failwith "eval_expr: deref" in
@@ -263,24 +422,69 @@ let get_previous_values = fun msg_name ->
let record_values = fun msg_name values ->
Hashtbl.replace last_values msg_name values
-(**Send an ivy message if needed: new values and/or 'on_event' condition true*)
-let execute_action = fun ac_id inputs buttons axis message ->
+let second_list (_,x) = x
+let first_list (x,_) = x
+
+(** add a leaf *)
+let trim_save_add_leaf = fun x channel_pair ->
+ let chan_name = first_list channel_pair in
+ let channel = second_list channel_pair in
+ match channel with
+ Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf "" chan_name trim.contents)
+ | Button i -> Printf.printf "%d" i
+
+(** save trim settings to file *)
+let trim_save = fun inputs ->
+ let xmlstring = ref "" in
+ List.iter (trim_save_add_leaf xmlstring) inputs;
+ xmlstring := xmlstring.contents ^ "";
+ let x = Xml.parse_string xmlstring.contents in
+ let pretty_xml_string = Xml.to_string_fmt x in
+ let output_trim_file = open_out trim_file_name.contents in
+ output_string output_trim_file pretty_xml_string;
+ close_out output_trim_file
+
+(** Adjust the trim on a specified channel *)
+let trim_adjust = fun axis_name adjustment inputs ->
+ let input = my_assoc axis_name inputs in
+ match input with
+ Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
+ | Button i -> failwith "No trim for buttons"
+
+(** Update variables state *)
+let update_variables = fun inputs buttons axis variables ->
+ List.iter (fun (_,var) ->
+ List.iter (fun (value, expr) ->
+ let event = eval_expr buttons axis inputs variables expr in
+ if event <> 0 then begin
+ (* remove and add again ? *)
+ var.value <- value
+ end
+ ) var.var_event
+ ) variables
+
+(** Send an ivy message if needed: new values and/or 'on_event' condition true*)
+let execute_action = fun ac_id inputs buttons axis variables message ->
let values =
List.map
- (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs expr)))
+ (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs variables expr)))
message.fields
and on_event =
match message.on_event with
None -> true
- | Some expr -> eval_expr buttons axis inputs expr <> 0 in
+ | Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
let previous_values = get_previous_values message.msg_name in
- if (on_event, values) <> previous_values && on_event then begin
+ (* FIXME ((value <> previous) && on_event) || send_always ??? *)
+ if ( ( (on_event, values) <> previous_values ) || message.send_always ) && on_event then begin
let vs = ("ac_id", Pprz.Int ac_id) :: values in
match message.msg_class with
"datalink" -> DL.message_send "input2ivy" message.msg_name vs
| "ground" -> G.message_send "input2ivy" message.msg_name vs
+ | "trim_plus" -> trim_adjust message.msg_name trim_step inputs
+ | "trim_minus" -> trim_adjust message.msg_name (-.trim_step) inputs
+ | "trim_save" -> trim_save inputs
| c -> failwith (sprintf "execute_action: unknown class '%s'" c)
end;
record_values message.msg_name (on_event, values)
@@ -288,18 +492,19 @@ let execute_action = fun ac_id inputs buttons axis message ->
(** Output on stderr the values from the input device *)
let print_inputs = fun nb_buttons buttons axis ->
- fprintf stderr "buttons: ";
+ fprintf Pervasives.stderr "buttons: ";
for i = 0 to nb_buttons - 1 do
- fprintf stderr "%d:%d " i (eval_input buttons axis (Button i))
+ fprintf Pervasives.stderr "%d:%d " i (eval_input buttons axis (Button i))
done;
- fprintf stderr "\naxis: ";
+ fprintf Pervasives.stderr "\naxis: ";
for i = 0 to Array.length axis - 1 do
- fprintf stderr "%d:%d " i (eval_input buttons axis (Axis (i, 0)))
+ fprintf Pervasives.stderr "%d:%d " i (eval_input buttons axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
done;
- fprintf stderr "\n%!"
+ fprintf Pervasives.stderr "\n%!"
-(** Get the values from the input values and send messages *)
+(** Get the values from the input values and send messages
+ This is called at a rate programmed in the xml *)
let execute_actions = fun actions ac_id ->
try
let (nb_buttons, buttons, axis) = stick_read () in
@@ -307,16 +512,43 @@ let execute_actions = fun actions ac_id ->
if !verbose then
print_inputs nb_buttons buttons axis;
- List.iter (execute_action ac_id actions.inputs buttons axis) actions.messages
+ (* TODO update variables before msg *)
+ update_variables actions.inputs buttons axis actions.variables;
+ List.iter (execute_action ac_id actions.inputs buttons axis actions.variables) actions.messages
with
exc -> prerr_endline (Printexc.to_string exc)
+(** process keyboard commands *)
+(** used for adjusting trims interactively from the keyboard *)
+(** this capability is mostly for bench-time trimming when a joystick does not have adequate buttons *)
+(** it is not a very complete capability *)
+let execute_kb_action = fun actions conditions ->
+ let ch = input_byte Pervasives.stdin in
+ (** esdx for left stick
+ ijkm for right *)
+
+ if true then begin
+ match ch with
+ 101 -> trim_adjust "ly" 1.0 actions.inputs
+ | 115 -> trim_adjust "lx" (-1.0) actions.inputs
+ | 100 -> trim_adjust "lx" 1.0 actions.inputs
+ | 120 -> trim_adjust "ly" (-1.0) actions.inputs
+ | 105 -> trim_adjust "ry" 1.0 actions.inputs
+ | 106 -> trim_adjust "rx" (-1.0) actions.inputs
+ | 107 -> trim_adjust "rx" 1.0 actions.inputs
+ | 109 -> trim_adjust "ry" (-1.0) actions.inputs
+ | _ -> trim_adjust "ly" 0.0 actions.inputs
+ end;
+
+ true
+
+
(************************************* MAIN **********************************)
let () =
-let ivy_bus = ref Defivybus.default_ivy_bus in
- let device_name = ref ""
+ let ivy_bus = ref Defivybus.default_ivy_bus in
+ let device_name = ref "/dev/input/js0"
and ac_name = ref "MYAC"
and xml_descr = ref "" in
@@ -326,6 +558,7 @@ let ivy_bus = ref Defivybus.default_ivy_bus in
"-ac", Arg.Set_string ac_name, "";
"-d", Arg.Set_string device_name, "";
"-v", Arg.Set verbose, "Verbose mode (useful to identify the channels of an input device)";
+ "-id", Arg.Set_int joystick_id, "Joystick ID, from 0-255. Each joystick requires a unique ID in a multiple joystick configuration.";
"-", Arg.String anon_fun, ""
]
and usage_msg = "Usage: " in
@@ -342,7 +575,13 @@ let ivy_bus = ref Defivybus.default_ivy_bus in
hash_index_of_settings !ac_name;
hash_index_of_blocks !ac_name;
- let actions = parse_descr !xml_descr in
+ Printf.printf "Joystick ID: %u\n" !joystick_id;
+
+ let joystick_conf_dir = conf_dir ^ "/joystick/" in
+ let xml_descr_full = joystick_conf_dir ^ !xml_descr in
+ trim_file_name := String.concat "." [xml_descr_full ; !ac_name ; "trim"];
+
+ let actions = parse_descr xml_descr_full trim_file_name.contents in
if stick_init !device_name <> 0 then
failwith (sprintf "Error: cannot open device %s\n" !device_name);
@@ -351,8 +590,15 @@ let ivy_bus = ref Defivybus.default_ivy_bus in
Ivy.init "Paparazzi joystick" "READY" (fun _ _ -> ());
Ivy.start !ivy_bus;
+ (** setup stdin *) (* TODO find a better way to change trim, use a GUI ? *)
+ (*let tstatus = (Unix.tcgetattr Unix.stdin) in
+ tstatus.c_icanon <- false;
+ Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;*)
+
ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
+ (*ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));*)
(** Start the main loop *)
let loop = Glib.Main.create true in
while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done
+
diff --git a/sw/ground_segment/joystick/usb_stick.c b/sw/ground_segment/joystick/usb_stick.c
index 87c7a3fab87..8d78c8b3166 100644
--- a/sw/ground_segment/joystick/usb_stick.c
+++ b/sw/ground_segment/joystick/usb_stick.c
@@ -21,6 +21,11 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
+/* 1/26/2011 - jpeverill added
+ support for joystick specific event interface. During testing it was found that many devices do not work properly with the standard event interface (such as the PS3 joystick). The joystick interface seems to work better. The old event interface is used if "event" is found in the name, otherwise the joystick interface is used.
+ now kills the process in the event of a read error (joystick unplugs)
+*/
+
//#define STICK_DBG 1
#include "usb_stick.h"
@@ -33,11 +38,14 @@
#include
#include
#include
-#include
#include
#include
#include
+//needed for joystick interface
+#include
+#include
+
#ifdef STICK_DBG
#define dbgprintf fprintf
#else
@@ -62,15 +70,19 @@
int stick_device_handle;
int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
-int16_t stick_button_values = 0;
+int32_t stick_button_values = 0;
int axis_code[AXIS_COUNT];
int stick_axis_count = 0;
int button_code[BUTTON_COUNT];
int stick_button_count = 0;
+int hid_led_count = 0;
+
int32_t axis_min[AXIS_COUNT], axis_max[AXIS_COUNT];
+int event_mode = STICK_MODE_UNKNOWN;
+
struct stick_code_param_ stick_init_param = {
0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
@@ -79,17 +91,56 @@ struct stick_code_param_ stick_init_param = {
//struct ff_effect effect;
+void led_test( void ) {
+
+ struct input_event ev; /* the event */
+
+ int lednum;
+
+ /* we turn off all the LEDs to start */
+ ev.type = EV_LED;
+
+ while (1) {
+ for (lednum=0;lednum<32;lednum++) {
+ ev.code = lednum;
+ ev.value = 1;
+ write(stick_device_handle, &ev, sizeof(struct input_event));
+ }
+
+ usleep(1000000);
+
+ for (lednum=0;lednum<32;lednum++) {
+ ev.code = lednum;
+ ev.value = 0;
+ write(stick_device_handle, &ev, sizeof(struct input_event));
+ }
+ }
+
+
+}
+
+
int init_hid_device(char* device_name)
{
int cnt;
- unsigned long key_bits[32],abs_bits[32];
+ unsigned long buttons,axes;
+ unsigned long key_bits[32],abs_bits[32],led_bits[32];
// unsigned long ff_bits[32];
int valbuf[16];
- char name[256] = "Unknown";
+ char name[MAX_NAME_LENGTH] = "Unknown";
stick_button_count = 0;
stick_axis_count = 0;
+ /* Detect device mode, event interface or joystick */
+ if (strstr( device_name, "event" ) == 0 ) {
+ dbgprintf(stderr," Using Joystick interface\n");
+ event_mode = STICK_MODE_JOYSTICK;
+ } else {
+ dbgprintf(stderr," Using Event interface\n");
+ event_mode = STICK_MODE_EVENT;
+ }
+
/* Open event device read only (with write permission for ff) */
stick_device_handle = open(device_name,O_RDONLY|O_NONBLOCK);
if (stick_device_handle<0) {
@@ -98,12 +149,35 @@ int init_hid_device(char* device_name)
return(1);
}
+ /* LED testing */
+
+// led_test( );
+
+
/* Which buttons has the device? */
memset(key_bits,0,32*sizeof(unsigned long));
- if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
- dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
- strerror(errno),__FILE__,__LINE__);
- return(1);
+ memset(led_bits,0,32*sizeof(unsigned long));
+ if (event_mode == STICK_MODE_EVENT ) {
+ if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
+ dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ //read LED count
+ if (ioctl(stick_device_handle,EVIOCGLED(32*sizeof(led_bits)),led_bits)<0) {
+ dbgprintf(stderr,"ERROR: can not get LED bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ dbgprintf(stderr,"LEDs: %X",*led_bits);
+
+
+ } else {
+ if (ioctl(stick_device_handle,JSIOCGBUTTONS,&buttons)<0) {
+ dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
}
/* Store buttons */
@@ -118,13 +192,22 @@ int init_hid_device(char* device_name)
memcpy(button_code,stick_init_param.button_code,BUTTON_COUNT*sizeof(int));
}
else {
- for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
- if (TEST_BIT(cnt, key_bits)) {
+ if (event_mode == STICK_MODE_EVENT) {
+ for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
+ if (TEST_BIT(cnt, key_bits)) {
+ button_code[stick_button_count++] = cnt;
+ dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+ }
+ if (stick_button_count == BUTTON_COUNT) break;
+ }
+ } else {
+ for (cnt = 0; cnt < buttons; cnt++) {
button_code[stick_button_count++] = cnt;
dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+ if (stick_button_count == BUTTON_COUNT) break;
}
- if (stick_button_count == BUTTON_COUNT) break;
}
+
if (stick_button_count == 0) {
dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
}
@@ -132,10 +215,18 @@ int init_hid_device(char* device_name)
/* Which axis has the device? */
memset(abs_bits,0,32*sizeof(unsigned long));
- if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
- dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
- strerror(errno),__FILE__,__LINE__);
- return(1);
+ if (event_mode == STICK_MODE_EVENT ) {
+ if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
+ dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ } else {
+ if (ioctl(stick_device_handle,JSIOCGAXES,&axes)<0) {
+ dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
}
/* Store axis */
@@ -150,13 +241,22 @@ int init_hid_device(char* device_name)
memcpy(axis_code,stick_init_param.axis_code,AXIS_COUNT*sizeof(int));
}
else {
- for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
- if (TEST_BIT(cnt, abs_bits)) {
+ if(event_mode == STICK_MODE_EVENT) {
+ for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
+ if (TEST_BIT(cnt, abs_bits)) {
+ axis_code[stick_axis_count++] = cnt;
+ dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+ }
+ if (stick_axis_count == AXIS_COUNT) break;
+ }
+ } else {
+ for (cnt = 0; cnt < axes; cnt++) {
axis_code[stick_axis_count++] = cnt;
dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+ if (stick_axis_count == AXIS_COUNT) break;
}
- if (stick_axis_count == AXIS_COUNT) break;
}
+
// at least 2 axis are needed in auto detection
if (stick_axis_count < 2) {
dbgprintf(stderr,"ERROR: no suitable axis found [%s:%d]\n",__FILE__,__LINE__);
@@ -167,14 +267,21 @@ int init_hid_device(char* device_name)
/* Axis param */
for (cnt = 0; cnt < stick_axis_count; cnt++)
{
- /* get axis value range */
- if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
- dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
- cnt,strerror(errno),__FILE__,__LINE__);
- return(1);
- }
- axis_min[cnt]=valbuf[1];
- axis_max[cnt]=valbuf[2];
+ if (event_mode == STICK_MODE_EVENT ) {
+ /* get axis value range */
+ if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
+ dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
+ cnt,strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ axis_min[cnt]=valbuf[1];
+ axis_max[cnt]=valbuf[2];
+ } else {
+ // with joystick interface, all axes are signed 16 bit with full range
+ axis_min[cnt]=-32768;
+ axis_max[cnt]=32768;
+ }
+
if (axis_min[cnt]>=axis_max[cnt]) {
dbgprintf(stderr,"ERROR: bad axis %d value range (%d,%d) [%s:%d]\n",
cnt,axis_min[cnt],axis_max[cnt],__FILE__,__LINE__);
@@ -184,6 +291,9 @@ int init_hid_device(char* device_name)
cnt,axis_min[cnt],axis_max[cnt]);
}
+ //--------------------------------------------------
+ // force feedback, TBD feature
+ //--------------------------------------------------
#if 0
/* Now get some information about force feedback */
memset(ff_bits,0,32*sizeof(unsigned long));
@@ -245,7 +355,11 @@ int init_hid_device(char* device_name)
}
#endif
- ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+ if (event_mode == STICK_MODE_EVENT ) {
+ ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+ } else {
+ ioctl(stick_device_handle, JSIOCGNAME(MAX_NAME_LENGTH), name);
+ }
printf("Input device name: \"%s\" on device \"%s\"\n", name, device_name);
return(0);
@@ -256,61 +370,79 @@ int stick_read( void ) {
int cnt;
struct input_event event;
+ struct js_event jsevent;
+
+ if (event_mode == STICK_MODE_EVENT ) {
+ /* Get events */
+ while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
+
+ switch (event.type) {
+ case EV_KEY:
+ for (cnt = 0; cnt < stick_button_count; cnt++) {
+ if (event.code == button_code[cnt]) {
+ if (event.value) stick_button_values |= (1 << cnt); // Set bit
+ else stick_button_values &= ~(1 << cnt); // Clear bit
+ break;
+ }
+ }
+ break;
+ case EV_ABS:
+ for (cnt = 0; cnt < stick_axis_count; cnt++) {
+ if (event.code == axis_code[cnt]) {
+ stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
+ break;
+ }
+ }
+ break;
+ default: break;
+ }
+ }
- /* Get events */
- while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
+ } else {
- switch (event.type) {
- case EV_KEY:
- for (cnt = 0; cnt < stick_button_count; cnt++) {
- if (event.code == button_code[cnt]) {
- if (event.value) stick_button_values |= (1 << cnt); // Set bit
- else stick_button_values &= ~(1 << cnt); // Clear bit
- break;
+ /* Get joystick events */
+ while (read(stick_device_handle,&jsevent,sizeof(jsevent))==sizeof(jsevent)) {
+
+ switch (jsevent.type) {
+ case JS_EVENT_BUTTON:
+ for (cnt = 0; cnt < stick_button_count; cnt++) {
+ if (jsevent.number == button_code[cnt]) {
+ if (jsevent.value) stick_button_values |= (1 << cnt); // Set bit
+ else stick_button_values &= ~(1 << cnt); // Clear bit
+ break;
+ }
}
- }
- break;
- case EV_ABS:
- for (cnt = 0; cnt < stick_axis_count; cnt++) {
- if (event.code == axis_code[cnt]) {
- stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
- break;
+ break;
+ case JS_EVENT_AXIS:
+ for (cnt = 0; cnt < stick_axis_count; cnt++) {
+ if (jsevent.number == axis_code[cnt]) {
+ stick_axis_values[cnt] = (( (jsevent.value - axis_min[cnt]) * ABS_MAX_VALUE ) / (axis_max[cnt] - axis_min[cnt])) - ABS_MID_VALUE;
+ break;
+ }
}
- }
- break;
- default: break;
+ break;
+ default: break;
+ }
}
}
- dbgprintf(stderr,"buttons %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d | ",
- (stick_button_values >> 0) & 1,
- (stick_button_values >> 1) & 1,
- (stick_button_values >> 2) & 1,
- (stick_button_values >> 3) & 1,
- (stick_button_values >> 4) & 1,
- (stick_button_values >> 5) & 1,
- (stick_button_values >> 6) & 1,
- (stick_button_values >> 7) & 1,
- (stick_button_values >> 8) & 1,
- (stick_button_values >> 9) & 1,
- (stick_button_values >> 10) & 1,
- (stick_button_values >> 11) & 1,
- (stick_button_values >> 12) & 1,
- (stick_button_values >> 13) & 1,
- (stick_button_values >> 14) & 1,
- (stick_button_values >> 15) & 1);
- dbgprintf(stderr,"axis %d %d %d %d %d %d %d %d %d %d\n",
- stick_axis_values[0],
- stick_axis_values[1],
- stick_axis_values[2],
- stick_axis_values[3],
- stick_axis_values[4],
- stick_axis_values[5],
- stick_axis_values[6],
- stick_axis_values[7],
- stick_axis_values[8],
- stick_axis_values[9]);
+ if (errno != EAGAIN) {
+ printf("Joystick read error!\n");
+ exit(1);
+ }
+
+ dbgprintf(stderr,"buttons ");
+ for (cnt = 0; cnt < stick_button_count; cnt++) {
+ dbgprintf(stderr,"%d ",(stick_button_values >> cnt) & 1 );
+ }
+
+ dbgprintf(stderr,"| axes ");
+ for (cnt = 0; cnt < stick_axis_count; cnt++) {
+ dbgprintf(stderr,"%d ",stick_axis_values[cnt]);
+ }
+
+ dbgprintf(stderr,"\n");
return 0;
}
@@ -318,7 +450,7 @@ int stick_read( void ) {
int stick_init( char * device_name ) {
- char devname[256];
+ char devname[MAX_NAME_LENGTH];
int cnt = 0;
/* test device_name, else look for a suitable device */
diff --git a/sw/ground_segment/joystick/usb_stick.h b/sw/ground_segment/joystick/usb_stick.h
index 65e709219ee..babde27e4da 100644
--- a/sw/ground_segment/joystick/usb_stick.h
+++ b/sw/ground_segment/joystick/usb_stick.h
@@ -27,17 +27,26 @@
#include
/* Max number of axis and buttons */
-#define STICK_BUTTON_COUNT 16
-#define STICK_AXIS_COUNT 10
+/* Increased, many new controllers have pressure sensitive buttons that show up as axes */
+#define STICK_BUTTON_COUNT 32
+#define STICK_AXIS_COUNT 32
/* Default values for the options */
#define STICK_INPUT_DEV_MAX 15
#define STICK_DEVICE_NAME "/dev/input/event"
+/* Event mode switched */
+#define STICK_MODE_UNKNOWN 0
+#define STICK_MODE_EVENT 1
+#define STICK_MODE_JOYSTICK 2
+
+/* Max name length */
+#define MAX_NAME_LENGTH 255
+
/* Global variables about the initialized device */
extern int stick_device_handle;
extern int8_t stick_axis_values[STICK_AXIS_COUNT];
-extern int16_t stick_button_values;
+extern int32_t stick_button_values;
extern int stick_axis_count, stick_button_count;
/* Structure for custom configuration
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
new file mode 100644
index 00000000000..096dbdfaaa9
--- /dev/null
+++ b/sw/ground_segment/misc/Makefile
@@ -0,0 +1,16 @@
+UNAME = $(shell uname -s)
+
+ifeq ("$(UNAME)","Darwin")
+ LIBRARYS = -L/opt/local/lib
+else
+ LIBRARYS = -s
+endif
+
+
+all: davis2ivy
+
+davis2ivy: davis2ivy.o
+ g++ -o davis2ivy davis2ivy.o $(LIBRARYS) -livy
+
+%.o : %.c
+ gcc -c -O2 -Wall -I /opt/local/include/ $<
diff --git a/sw/ground_segment/misc/davis2ivy.c b/sw/ground_segment/misc/davis2ivy.c
new file mode 100644
index 00000000000..7e5c70aa344
--- /dev/null
+++ b/sw/ground_segment/misc/davis2ivy.c
@@ -0,0 +1,274 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2011 Andreas Gaeb
+ *
+ * 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 davis2ivy.c
+ * \brief Connect a Davis VantagePro weather station to the Paparazzi system
+ *
+ * The program communicates with a Davis VantagePro(2) weather station connected
+ * to a serial port. It asks for new data (Davis' LOOP command) in the
+ * specified intervals, extracts the relevant data (ambient pressure and
+ * temperature, wind speed and direction) and broadcasts this via the Ivy bus.
+ *
+ * At the moment, the Ivy messages should be sent with the ID of the actually
+ * flying aircraft, which integrates them into the log file, as long as the
+ * aircraft sends its alive message.
+ *
+ * Useful links:
+ * - Weather Stations
+ * - Communication docs
+ *
+ */
+
+
+ #include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+
+
+typedef enum { FALSE = 0, TRUE } BOOL;
+
+#define PACKET_LENGTH 99
+
+
+// global variables
+int fd, ac_id = 1;
+const char *device;
+unsigned char packet[PACKET_LENGTH];
+TimerId tid;
+BOOL want_alive_msg = FALSE;
+
+
+/// Handler for Ctrl-C, exits the main loop
+void sigint_handler(int sig) {
+ IvyStop();
+ TimerRemove(tid);
+ close(fd);
+}
+
+/// open the serial port with the appropiate settings
+void open_port(const char* device) {
+ fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
+ if (fd == -1) {
+ fprintf(stderr, "open_port: unable to open device %s - ", device);
+ perror(NULL);
+ exit(EXIT_FAILURE);
+ }
+ // setup connection options
+ struct termios options;
+
+ // get the current options
+ tcgetattr(fd, &options);
+
+ // set local mode, enable receiver, set comm. options:
+ // 8 data bits, 1 stop bit, no parity, 19200 Baud
+ options.c_cflag = CLOCAL | CREAD | CS8 | B19200;
+
+ // write options back to port
+ tcsetattr(fd, TCSANOW, &options);
+
+}
+
+/// disable transactions and empty queue
+void reset_station() {
+ char newline = '\n', bytes = 0;
+ fprintf(stderr, "Resetting communication\n");
+ // send a \n (wakeup and cancel all running transmits)
+ bytes = write(fd, &newline, 1);
+ // read and discard everything that might be left in the queue
+ close(fd);
+ sleep(1);
+ open_port(device);
+}
+
+/// send a wakeup call to the station
+BOOL wakeup(int tries) {
+ int loops = tries, bytes;
+ BOOL woken = FALSE;
+ char buf[] = {0, 0};
+ char newline = '\n';
+ do {
+ // send a \n
+ bytes = write(fd, &newline, 1);
+ // wait until station answers with \n\r
+ usleep(30000);
+ bytes = read(fd, buf, sizeof(buf));
+ woken = (buf[0] == 10) && (buf[1] == 13);
+ } while (!woken && loops-- > 0);
+ if (!woken) {
+ fprintf(stderr, "Could not wake up station: ");
+ if (bytes < 1) fprintf(stderr, "no bytes received\n");
+ else fprintf(stderr, "received %02x:%02x instead of \\n\\r\n", buf[0], buf[1]);
+ reset_station();
+ }
+ return woken;
+}
+
+/// send a LOOP command (read sensor data) to the station and get the packet back
+BOOL send_loop() {
+ char msg[32], ack;
+ // TODO maybe ask for more packets?
+ snprintf(msg, sizeof(msg), "LOOP %i\n", 1);
+ int bytes = write(fd, msg, strlen(msg));
+ usleep(120000);
+ bytes = read(fd, &ack, 1);
+ if (bytes < 1 || ack != 0x06) {
+ fprintf(stderr, "Failed to receive ACK from station\n");
+ reset_station();
+ return FALSE;
+ }
+ bytes = read(fd, packet, PACKET_LENGTH);
+ if (bytes < PACKET_LENGTH) {
+ fprintf(stderr, "Received packet is incomplete, only %i of %i bytes\n",
+ bytes, PACKET_LENGTH);
+ reset_station();
+ return FALSE;
+ } else {
+ return TRUE;
+ }
+}
+
+/// get the relevant data from the packet and sent it as Ivy message
+void decode_and_send_to_ivy() {
+
+ // check packet integrity
+ char expected[] = "LOO";
+ if (strncmp((char *)packet, expected, 3) != 0) {
+ fprintf(stderr, "Received packet from the weather station which does not match the expected format\n");
+ reset_station();
+ return;
+ }
+
+ // TODO CRC checking (is rather involved for the Davis protocol)
+
+ // get relevant data and convert to SI units
+ // see chapter IX.1 of the protocol definition
+ float
+ pstatic_Pa = (packet[7] | packet[8] << 8)*3.386388640341, // original is inches Hg / 1000
+ temp_degC = ((packet[12] | packet[13] << 8)/10.0 - 32.0)*5.0/9.0, // original is deg F / 10
+ windspeed_mps = packet[14]*0.44704, // original is miles per hour
+ winddir_deg = packet[16] | packet[17] << 8;
+
+
+ // TODO get the real MD5 for the aircraft id
+ if (want_alive_msg)
+ IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
+
+ // format has to match declaration in conf/messages.xml
+ IvySendMsg("%d WEATHER %f %f %f %f\n",
+ ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg);
+}
+
+/// Get data from the station and send it via Ivy
+/** This function is executed by the timer
+ */
+void handle_timer (TimerId id, void *data, unsigned long delta) {
+ if (wakeup(3) && send_loop()) decode_and_send_to_ivy();
+}
+
+void print_usage(int argc, char ** argv) {
+ fprintf(stderr, "Usage: %s [-a] [-b ] [-d ] [-i ] [-s ]\n",
+ argv[0]);
+};
+
+/// Main function
+int main(int argc, char **argv) {
+ // default values for options
+ const char
+ *defaultbus = "127.255.255.255:2010",
+ *bus = defaultbus,
+ *defaultdevice = "/dev/ttyUSB1";
+ device = defaultdevice;
+ long delay = 1000;
+
+ // parse options
+ char c;
+ while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) {
+ switch (c) {
+ case 'h':
+ print_usage(argc, argv);
+ exit(EXIT_SUCCESS);
+ break;
+ case 'a':
+ want_alive_msg = TRUE;
+ break;
+ case 'b':
+ bus = optarg;
+ break;
+ case 'd':
+ device = optarg;
+ break;
+ case 'i':
+ ac_id = atoi(optarg);
+ break;
+ case 's':
+ delay = atoi(optarg)*1000;
+ break;
+ case '?':
+ if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
+ fprintf (stderr, "Option -%c requires an argument.\n", optopt);
+ else if (isprint (optopt))
+ fprintf (stderr, "Unknown option `-%c'.\n", optopt);
+ else
+ fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
+ print_usage(argc, argv);
+ exit(EXIT_FAILURE);
+ default:
+ abort ();
+ }
+ }
+
+
+ // make Ctrl-C stop the main loop and clean up properly
+ signal(SIGINT, sigint_handler);
+
+ bzero (packet, PACKET_LENGTH);
+ open_port(device);
+
+ // setup Ivy communication
+ IvyInit("davis2ivy", "READY", 0, 0, 0, 0);
+ IvyStart(bus);
+
+ // create timer
+ tid = TimerRepeatAfter (0, delay, handle_timer, 0);
+
+#if IVYMINOR_VERSION == 8
+ IvyMainLoop (NULL,NULL);
+#else
+ IvyMainLoop ();
+#endif
+
+ return 0;
+}
diff --git a/sw/ground_segment/misc/readme.txt b/sw/ground_segment/misc/readme.txt
new file mode 100644
index 00000000000..5221c38c9f3
--- /dev/null
+++ b/sw/ground_segment/misc/readme.txt
@@ -0,0 +1,3 @@
+davis2ivy:
+ A wrapper to communicate with a Davis VantagePro/VantagePro2 weather
+ station and integrate weather data into the telemetry link.
diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml
index 774b7f1ea7f..ba2c64c93a3 100644
--- a/sw/lib/ocaml/pprz.ml
+++ b/sw/lib/ocaml/pprz.ml
@@ -168,6 +168,8 @@ let alt_unit_coef_of_xml = function xml ->
| ("rad", "deg") | ("rad/s", "deg/s") -> string_of_float (180. /. pi)
| ("m", "cm") | ("m/s", "cm/s") -> "100."
| ("cm", "m") | ("cm/s", "m/s") -> "0.01"
+ | ("m", "mm") | ("m/s", "mm/s") -> "1000."
+ | ("mm", "m") | ("mm/s", "m/s") -> "0.001"
| ("decideg", "deg") -> "0.1"
| (_, _) -> "1."
diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli
index 855dd2938cf..b52124d3d55 100644
--- a/sw/lib/ocaml/pprz.mli
+++ b/sw/lib/ocaml/pprz.mli
@@ -93,8 +93,12 @@ val alt_unit_coef_of_xml : Xml.xml -> string
rad -> deg
m -> cm
cm -> m
+ m -> mm
+ mm -> m
m/s -> cm/s
cm/s -> m/s
+ m/s -> mm/s
+ mm/s -> m/s
decideg -> deg
*)