diff --git a/CHANGELOG.md b/CHANGELOG.md
index 97e05bc455..59d90f1ad1 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,11 +1,86 @@
-Paparazzi 4.9 - development branch
-==================================
+Paparazzi 5.0.0_stable
+======================
+
+Stable version release
+
+General
+-------
- STM libs completely replaced by libopencm3
+- [gcc-arm-embedded] (https://launchpad.net/gcc-arm-embedded) is the new recommended toolchain
+- Use findlib (ocamlfind) for ocaml packages. Faster build.
+ [#274] (https://github.com/paparazzi/paparazzi/pull/274)
+- Building/Running the groundsegment on an ARM (e.g. RaspberryPi).
- Input2ivy uses SDL for joysticks (cross-platform, works on OSX as well now)
[#220] (https://github.com/paparazzi/paparazzi/pull/220)
- Option to change text papget color using a combobox
[#194] (https://github.com/paparazzi/paparazzi/pull/194)
+- Redundant communications
+ [#429] (https://github.com/paparazzi/paparazzi/pull/429)
+- Log also contains includes like procedures now, so replay if these missions is possible.
+ [#227] (https://github.com/paparazzi/paparazzi/issues/227)
+- Paparazzi Center
+ - New simulation launcher with dialog and detection of available ones.
+ [#354] (https://github.com/paparazzi/paparazzi/pull/354)
+ - Checkbox to print extra configuration information during build.
+- GCS:
+ - Fix panning with mouse if there are no background tiles.
+ [#9] (https://github.com/paparazzi/paparazzi/issues/9)
+ - Higher zoom level for maps.
+ [#277] (https://github.com/paparazzi/paparazzi/issues/277)
+
+Hardware support
+----------------
+
+- initial support for STM32F4
+ - Apogee autopilot
+ - KroozSD autopilot
+- Parrot AR Drone 2 support: raw and sdk versions
+- CH Robotics UM6 IMU/AHRS
+- GPS/INS XSens Mti-G support
+- GPS Sirf support
+- GPS Skytraq now usable for fixedwings as well
+ [#167] (https://github.com/paparazzi/paparazzi/issues/167)
+- Mikrokopter V2 BLDC
+ [#377] (https://github.com/paparazzi/paparazzi/pull/377)
+- PX4Flow sensor
+ [#379] (https://github.com/paparazzi/paparazzi/pull/379)
+- Dropped AVR support
+
+Airborne
+--------
+
+- State interface with automatic coordinate transformations
+ [#237] (https://github.com/paparazzi/paparazzi/pull/237)
+- New AHRS filter: Multiplicative quaternion linearized Kalman Filter
+- New SPI driver with transaction queues.
+ - Fix transactions with zero length input.
+ [#348] (https://github.com/paparazzi/paparazzi/issues/348)
+- Peripherals: Cleanup and refactoring.
+ - MPU60x0 peripheral supporting SPI and I2C with slave.
+- UDP datalink.
+- Magnetometer current offset calibration.
+ [#346] (https://github.com/paparazzi/paparazzi/pull/346)
+- Gain scheduling module.
+ [#335] (https://github.com/paparazzi/paparazzi/pull/335)
+
+Rotorcraft firmware specific
+----------------------------
+
+- Quadshot transitioning vehicle support.
+- Care Free Mode
+
+
+Paparazzi 4.2.1_stable
+======================
+
+Maintenance release
+
+- fix elf PT_LOAD type in lpc21iap LPC USB download
+- fix electrical.current estimate in sim
+- fix LPC+xbee_api in rotorcraft
+- fix conversion of vsupply to decivolts if offset is used
+- more robust dfu flash script, only upload to Lisa/M
Paparazzi 4.2.0_stable
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index b5efec74a6..2dce3cc671 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -37,7 +37,7 @@
-
+
@@ -133,7 +133,6 @@
-
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 0604b4a152..472c32d189 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -47,7 +47,7 @@
-
+
@@ -102,13 +102,13 @@
-
-
-
+
+
+
-
-
-
+
+
+
@@ -206,9 +206,10 @@
+
-
-
+
+
diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml
index c7bb880258..dfbcab0f90 100644
--- a/conf/airframes/ardrone2_sdk.xml
+++ b/conf/airframes/ardrone2_sdk.xml
@@ -14,7 +14,7 @@
-
+
@@ -97,6 +97,7 @@
+
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index 1f4331d59a..e9e9aa9dba 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -32,7 +32,7 @@
-
+
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml
new file mode 100644
index 0000000000..c4f1066f5f
--- /dev/null
+++ b/conf/airframes/examples/quadrotor_mlkf.xml
@@ -0,0 +1,228 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example
index 6e36df42b1..0c69ef79c8 100644
--- a/conf/control_panel.xml.example
+++ b/conf/control_panel.xml.example
@@ -83,28 +83,12 @@
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index c4624667f1..2e5c837a16 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -22,7 +22,7 @@ nps.MAKEFILE = nps
nps.CFLAGS += -DSITL -DUSE_NPS
nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags)
-nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lgsl -lgslcblas
+nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lpcre -lgsl -lgslcblas
nps.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps
nps.LDFLAGS += $(shell sdl-config --libs)
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
new file mode 100644
index 0000000000..fad4a3919f
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
@@ -0,0 +1,20 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# ARDrone 2 Flightrecorder GPS unit
+
+
+ap.CFLAGS += -DUSE_GPS -DUSE_GPS_ARDRONE2
+
+ifneq ($(GPS_LED),none)
+ ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ardrone2.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
index 446b84af59..1e16703269 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile
@@ -39,32 +39,37 @@
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
- IMU_ASPIRIN_CFLAGS = -DUSE_IMU
+ IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
endif
-IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
-IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
- $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
+IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\"
+IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c
+IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
+IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
+
+# Magnetometer
+#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
include $(CFG_SHARED)/spi_master.makefile
ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
endif
-IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
-
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
-ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
-ap.srcs += $(IMU_ASPIRIN_SRCS)
+ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
+ap.srcs += $(IMU_ASPIRIN_2_SRCS)
#
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
similarity index 69%
rename from conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
rename to conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
index 3ad6e9a953..446b84af59 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_new.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile
@@ -39,39 +39,32 @@
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
- IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
+ IMU_ASPIRIN_CFLAGS = -DUSE_IMU
endif
-IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2.h\"
-IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
-IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2.c
-IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
-IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
-
-# Magnetometer
-#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c
+IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
+IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
include $(CFG_SHARED)/spi_master.makefile
ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
-IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
-IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
+IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
+IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
+IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
+IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
endif
-#IMU_ASPIRIN_2_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
+IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
-ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
-ap.srcs += $(IMU_ASPIRIN_2_SRCS)
+ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
+ap.srcs += $(IMU_ASPIRIN_SRCS)
#
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
index 8dfb93613a..99968ac5a2 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
@@ -2,11 +2,13 @@
#
# Aspirin IMU v2.2
#
+# actually identical with v2.1 since baro is not read in IMU driver
+#
#
# required xml:
#
#
-#
+#
#
#
#
@@ -37,37 +39,4 @@
#
-# for fixedwing firmware and ap only
-ifeq ($(TARGET), ap)
- IMU_ASPIRIN_CFLAGS = -DUSE_IMU
-endif
-
-IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\"
-IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
- $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c
-
-include $(CFG_SHARED)/spi_master.makefile
-
-ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
-else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
-# Slave select configuration
-# SLAVE2 is on PB12 (NSS) (MPU600 CS)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
-endif
-
-IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_2
-
-# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
-# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
-
-ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
-ap.srcs += $(IMU_ASPIRIN_SRCS)
-
-
-#
-# NPS simulator
-#
-include $(CFG_SHARED)/imu_nps.makefile
+include $(CFG_SHARED)/imu_aspirin_v2.1.makefile
diff --git a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
new file mode 100644
index 0000000000..b41ec9b741
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
@@ -0,0 +1,81 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Drotek 10DOF V2 IMU via I2C
+#
+#
+# required xml:
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+
+# for fixedwing firmware and ap only
+ifeq ($(TARGET), ap)
+ IMU_DROTEK_2_CFLAGS = -DUSE_IMU
+endif
+
+IMU_DROTEK_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_drotek_10dof_v2.h\"
+IMU_DROTEK_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_DROTEK_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_drotek_10dof_v2.c
+IMU_DROTEK_2_SRCS += peripherals/mpu60x0.c
+IMU_DROTEK_2_SRCS += peripherals/mpu60x0_i2c.c
+
+# Magnetometer
+IMU_DROTEK_2_SRCS += peripherals/hmc58xx.c
+
+
+# set default i2c bus
+ifndef DROTEK_2_I2C_DEV
+ifeq ($(ARCH), lpc21)
+DROTEK_2_I2C_DEV=i2c0
+else ifeq ($(ARCH), stm32)
+DROTEK_2_I2C_DEV=i2c2
+endif
+endif
+
+# convert i2cx to upper case
+DROTEK_2_I2C_DEV_UPPER=$(shell echo $(DROTEK_2_I2C_DEV) | tr a-z A-Z)
+
+IMU_DROTEK_2_CFLAGS += -DDROTEK_2_I2C_DEV=$(DROTEK_2_I2C_DEV)
+IMU_DROTEK_2_CFLAGS += -DUSE_$(DROTEK_2_I2C_DEV_UPPER)
+
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+
+ap.CFLAGS += $(IMU_DROTEK_2_CFLAGS)
+ap.srcs += $(IMU_DROTEK_2_SRCS)
+
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/joystick/ms_sidewinder.xml b/conf/joystick/ms_sidewinder.xml
index 29c61b7734..d439e422e8 100644
--- a/conf/joystick/ms_sidewinder.xml
+++ b/conf/joystick/ms_sidewinder.xml
@@ -4,8 +4,6 @@
axis 1: pitch
axis 2: yaw
axis 3: throttle (reversed)
- axis 4: hat switch left/right (right is positive)
- axis 5: hat switch up/down (down is positive)
It has 9 buttons.
b0 - fire
@@ -18,6 +16,11 @@ It has 9 buttons.
b7 - button D
b8 - shift button
+and a POV hat.
+You can use the Hat() function to trigger events,
+where is one of
+Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
+so e.g. HatDown(hat)
-->
@@ -26,8 +29,6 @@ It has 9 buttons.
-
-
@@ -37,22 +38,40 @@ It has 9 buttons.
+
+
+
+
+
+
+
+
+
-
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/xbox_gamepad.xml b/conf/joystick/xbox_gamepad.xml
new file mode 100644
index 0000000000..10e556c096
--- /dev/null
+++ b/conf/joystick/xbox_gamepad.xml
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/airspeed_amsys.xml b/conf/modules/airspeed_amsys.xml
index bc1d68ebc1..88c3faf9de 100644
--- a/conf/modules/airspeed_amsys.xml
+++ b/conf/modules/airspeed_amsys.xml
@@ -8,10 +8,11 @@
(AMS 5812-0003-D)
-
+
+
-
+
diff --git a/conf/settings/control/stabilization_att_float_quat.xml b/conf/settings/control/stabilization_att_float_quat.xml
new file mode 100644
index 0000000000..e2e3c886e8
--- /dev/null
+++ b/conf/settings/control/stabilization_att_float_quat.xml
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/system/preferences.d/paparazzi-uav-ppa b/conf/system/preferences.d/paparazzi-uav-ppa
new file mode 100644
index 0000000000..0509d67510
--- /dev/null
+++ b/conf/system/preferences.d/paparazzi-uav-ppa
@@ -0,0 +1,3 @@
+Package: lpc21isp
+Pin: version 1.48*
+Pin-Priority: 1001
diff --git a/paparazzi_version b/paparazzi_version
index ad00a1aa0c..0fde2c0289 100755
--- a/paparazzi_version
+++ b/paparazzi_version
@@ -1,6 +1,6 @@
#!/bin/sh
-DEF_VER=v4.9_devel
+DEF_VER=v5.0.0_stable
# First try git describe (if running on a git repo),
# then use default version from above (for release tarballs).
diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c
index 60134d66ca..252ba5fa09 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c
@@ -382,6 +382,7 @@ __attribute__ ((always_inline)) static inline void SpiSlaveAutomaton(struct spi_
#if SPI_MASTER
#if USE_SPI0
+#error "SPI0 is currently not implemented in the mcu_periph/spi HAL for the LPC!"
// void spi0_ISR(void) __attribute__((naked));
//
diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c
index d5d8be1b28..8066250927 100644
--- a/sw/airborne/arch/stm32/mcu_arch.c
+++ b/sw/airborne/arch/stm32/mcu_arch.c
@@ -33,11 +33,7 @@
#include
#include
#include
-#if defined(STM32F1)
-#include
-#elif defined(STM32F4)
-#include
-#endif
+#include
#include
#include "std.h"
diff --git a/sw/airborne/arch/stm32/mcu_periph/can_arch.c b/sw/airborne/arch/stm32/mcu_periph/can_arch.c
index ab91e8e0b1..44702dc19b 100644
--- a/sw/airborne/arch/stm32/mcu_periph/can_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/can_arch.c
@@ -166,9 +166,9 @@ int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len)
void usb_lp_can_rx0_isr(void)
{
- u32 id, fmi;
+ uint32_t id, fmi;
bool ext, rtr;
- u8 length, data[8];
+ uint8_t length, data[8];
can_receive(CAN1,
0, /* FIFO: 0 */
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
index e4024e94e9..902d915759 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
@@ -71,7 +71,7 @@ static inline void __enable_irq(void) { asm volatile ("cpsie i"); }
#define __I2C_REG_CRITICAL_ZONE_STOP __enable_irq();
-static inline void PPRZ_I2C_SEND_STOP(u32 i2c)
+static inline void PPRZ_I2C_SEND_STOP(uint32_t i2c)
{
// Man: p722: Stop generation after the current byte transfer or after the current Start condition is sent.
I2C_CR1(i2c) |= I2C_CR1_STOP;
@@ -88,7 +88,7 @@ static inline void PPRZ_I2C_SEND_STOP(u32 i2c)
static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph)
{
- u32 i2c = (u32) periph->reg_addr;
+ uint32_t i2c = (uint32_t) periph->reg_addr;
// Reset the buffer pointer to the first byte
periph->idx_buf = 0;
@@ -134,7 +134,7 @@ enum STMI2CSubTransactionStatus {
// Doc ID 13902 Rev 11 p 710/1072
// Transfer Sequence Diagram for Master Transmitter
-static inline enum STMI2CSubTransactionStatus stmi2c_send(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
+static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
{
uint16_t SR1 = I2C_SR1(i2c);
@@ -215,7 +215,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(u32 i2c, struct i2c_pe
// Doc ID 13902 Rev 11 p 714/1072
// Transfer Sequence Diagram for Master Receiver for N=1
-static inline enum STMI2CSubTransactionStatus stmi2c_read1(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
+static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
{
uint16_t SR1 = I2C_SR1(i2c);
@@ -278,7 +278,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(u32 i2c, struct i2c_p
// Doc ID 13902 Rev 11 p 713/1072
// Transfer Sequence Diagram for Master Receiver for N=2
-static inline enum STMI2CSubTransactionStatus stmi2c_read2(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
+static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
{
uint16_t SR1 = I2C_SR1(i2c);
@@ -348,7 +348,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(u32 i2c, struct i2c_p
// Doc ID 13902 Rev 11 p 712/1072
// Transfer Sequence Diagram for Master Receiver for N>2
-static inline enum STMI2CSubTransactionStatus stmi2c_readmany(u32 i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
+static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans)
{
uint16_t SR1 = I2C_SR1(i2c);
@@ -473,51 +473,51 @@ static inline void i2c_error(struct i2c_periph *periph)
uint8_t err_nr = 0;
#endif
periph->errors->er_irq_cnt;
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_AF) != 0) { /* Acknowledge failure */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_AF) != 0) { /* Acknowledge failure */
periph->errors->ack_fail_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_AF;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_AF;
#ifdef I2C_DEBUG_LED
err_nr = 1;
#endif
}
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_BERR) != 0) { /* Misplaced Start or Stop condition */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_BERR) != 0) { /* Misplaced Start or Stop condition */
periph->errors->miss_start_stop_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_BERR;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_BERR;
#ifdef I2C_DEBUG_LED
err_nr = 2;
#endif
}
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_ARLO) != 0) { /* Arbitration lost */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_ARLO) != 0) { /* Arbitration lost */
periph->errors->arb_lost_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_ARLO;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_ARLO;
#ifdef I2C_DEBUG_LED
err_nr = 3;
#endif
}
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_OVR) != 0) { /* Overrun/Underrun */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_OVR) != 0) { /* Overrun/Underrun */
periph->errors->over_under_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_OVR;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_OVR;
#ifdef I2C_DEBUG_LED
err_nr = 4;
#endif
}
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_PECERR) != 0) { /* PEC Error in reception */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_PECERR) != 0) { /* PEC Error in reception */
periph->errors->pec_recep_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_PECERR;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_PECERR;
#ifdef I2C_DEBUG_LED
err_nr = 5;
#endif
}
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_TIMEOUT) != 0) { /* Timeout or Tlow error */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_TIMEOUT) != 0) { /* Timeout or Tlow error */
periph->errors->timeout_tlow_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_TIMEOUT;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_TIMEOUT;
#ifdef I2C_DEBUG_LED
err_nr = 6;
#endif
}
- if ((I2C_SR1((u32)periph->reg_addr) & I2C_SR1_SMBALERT) != 0) { /* SMBus alert */
+ if ((I2C_SR1((uint32_t)periph->reg_addr) & I2C_SR1_SMBALERT) != 0) { /* SMBus alert */
periph->errors->smbus_alert_cnt++;
- I2C_SR1((u32)periph->reg_addr) &= ~I2C_SR1_SMBALERT;
+ I2C_SR1((uint32_t)periph->reg_addr) &= ~I2C_SR1_SMBALERT;
#ifdef I2C_DEBUG_LED
err_nr = 7;
#endif
@@ -531,7 +531,7 @@ static inline void i2c_error(struct i2c_periph *periph)
}
-static inline void stmi2c_clear_pending_interrupts(u32 i2c)
+static inline void stmi2c_clear_pending_interrupts(uint32_t i2c)
{
uint16_t SR1 = I2C_SR1(i2c);
@@ -648,7 +648,7 @@ static inline void i2c_irq(struct i2c_periph *periph)
#endif
// Save Some Direct Access to the I2C Registers ...
- u32 i2c = (u32) periph->reg_addr;
+ uint32_t i2c = (uint32_t) periph->reg_addr;
/////////////////////////////
// Check if we were ready ...
@@ -944,7 +944,7 @@ void i2c1_hw_init(void) {
}
void i2c1_ev_isr(void) {
- u32 i2c = (u32) i2c1.reg_addr;
+ uint32_t i2c = (uint32_t) i2c1.reg_addr;
I2C_CR2(i2c) &= ~I2C_CR2_ITERREN;
i2c_irq(&i2c1);
i2c1_watchdog_counter = 0;
@@ -952,7 +952,7 @@ void i2c1_ev_isr(void) {
}
void i2c1_er_isr(void) {
- u32 i2c = (u32) i2c1.reg_addr;
+ uint32_t i2c = (uint32_t) i2c1.reg_addr;
I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN;
i2c_irq(&i2c1);
i2c1_watchdog_counter = 0;
@@ -1033,7 +1033,7 @@ void i2c2_hw_init(void) {
}
void i2c2_ev_isr(void) {
- u32 i2c = (u32) i2c2.reg_addr;
+ uint32_t i2c = (uint32_t) i2c2.reg_addr;
I2C_CR2(i2c) &= ~I2C_CR2_ITERREN;
i2c_irq(&i2c2);
i2c2_watchdog_counter = 0;
@@ -1041,7 +1041,7 @@ void i2c2_ev_isr(void) {
}
void i2c2_er_isr(void) {
- u32 i2c = (u32) i2c2.reg_addr;
+ uint32_t i2c = (uint32_t) i2c2.reg_addr;
I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN;
i2c_irq(&i2c2);
i2c2_watchdog_counter = 0;
@@ -1121,7 +1121,7 @@ void i2c3_hw_init(void) {
}
void i2c3_ev_isr(void) {
- u32 i2c = (u32) i2c3.reg_addr;
+ uint32_t i2c = (uint32_t) i2c3.reg_addr;
I2C_CR2(i2c) &= ~I2C_CR2_ITERREN;
i2c_irq(&i2c3);
i2c3_watchdog_counter = 0;
@@ -1129,7 +1129,7 @@ void i2c3_ev_isr(void) {
}
void i2c3_er_isr(void) {
- u32 i2c = (u32) i2c3.reg_addr;
+ uint32_t i2c = (uint32_t) i2c3.reg_addr;
I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN;
i2c_irq(&i2c3);
i2c3_watchdog_counter = 0;
@@ -1151,7 +1151,7 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate)
volatile int devider;
volatile int risetime;
- u32 i2c = (u32) periph->reg_addr;
+ uint32_t i2c = (uint32_t) periph->reg_addr;
/*****************************************************
Bitrate:
@@ -1361,7 +1361,7 @@ bool_t i2c_idle(struct i2c_periph* periph)
// This is actually a difficult function:
// -simply reading the status flags can clear bits and corrupt the transaction
- u32 i2c = (u32) periph->reg_addr;
+ uint32_t i2c = (uint32_t) periph->reg_addr;
#ifdef I2C_DEBUG_LED
#ifdef USE_I2C1
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index 6aa01e8620..4384d15686 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -69,11 +69,11 @@
* Libopencm3 specifc communication parameters for a SPI peripheral in master mode.
*/
struct locm3_spi_comm {
- u32 br; ///< baudrate (clock divider)
- u32 cpol; ///< clock polarity
- u32 cpha; ///< clock phase
- u32 dff; ///< data frame format 8/16 bits
- u32 lsbfirst; ///< frame format lsb/msb first
+ uint32_t br; ///< baudrate (clock divider)
+ uint32_t cpol; ///< clock polarity
+ uint32_t cpha; ///< clock phase
+ uint32_t dff; ///< data frame format 8/16 bits
+ uint32_t lsbfirst; ///< frame format lsb/msb first
};
/**
@@ -81,19 +81,19 @@ struct locm3_spi_comm {
* which allows for more code reuse.
*/
struct spi_periph_dma {
- u32 spi; ///< SPI peripheral identifier
- u32 spidr; ///< SPI DataRegister address for DMA
- u32 dma; ///< DMA controller base address (DMA1 or DMA2)
- u8 rx_chan; ///< receive DMA channel number
- u8 tx_chan; ///< transmit DMA channel number
- u8 rx_nvic_irq; ///< receive interrupt
- u8 tx_nvic_irq; ///< transmit interrupt
- u16 tx_dummy_buf; ///< dummy tx buffer for receive only cases
+ uint32_t spi; ///< SPI peripheral identifier
+ uint32_t spidr; ///< SPI DataRegister address for DMA
+ uint32_t dma; ///< DMA controller base address (DMA1 or DMA2)
+ uint8_t rx_chan; ///< receive DMA channel number
+ uint8_t tx_chan; ///< transmit DMA channel number
+ uint8_t rx_nvic_irq; ///< receive interrupt
+ uint8_t tx_nvic_irq; ///< transmit interrupt
+ uint16_t tx_dummy_buf; ///< dummy tx buffer for receive only cases
bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len
- u16 rx_dummy_buf; ///< dummy rx buffer for receive only cases
+ uint16_t rx_dummy_buf; ///< dummy rx buffer for receive only cases
bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len
struct locm3_spi_comm comm; ///< current communication paramters
- u8 comm_sig; ///< comm config signature used to check for changes
+ uint8_t comm_sig; ///< comm config signature used to check for changes
};
@@ -112,8 +112,8 @@ static struct spi_periph_dma spi3_dma;
static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_transaction* _trans);
static void spi_next_transaction(struct spi_periph* periph);
-static void spi_configure_dma(u32 dma, u8 chan, u32 periph_addr, u32 buf_addr,
- u16 len, enum SPIDataSizeSelect dss, bool_t increment);
+static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr,
+ uint16_t len, enum SPIDataSizeSelect dss, bool_t increment);
static void process_rx_dma_interrupt(struct spi_periph* periph);
static void process_tx_dma_interrupt(struct spi_periph* periph);
static void spi_arch_int_enable(struct spi_periph *spi);
@@ -436,8 +436,8 @@ static void set_comm_from_transaction(struct locm3_spi_comm* c, struct spi_trans
* Helpers for SPI transactions with DMA
*
*****************************************************************************/
-static void spi_configure_dma(u32 dma, u8 chan, u32 periph_addr, u32 buf_addr,
- u16 len, enum SPIDataSizeSelect dss, bool_t increment)
+static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr,
+ uint16_t len, enum SPIDataSizeSelect dss, bool_t increment)
{
dma_channel_reset(dma, chan);
dma_set_peripheral_address(dma, chan, periph_addr);
@@ -519,12 +519,12 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
dma->comm_sig = sig;
/* apply the new configuration */
- spi_disable((u32)periph->reg_addr);
- spi_init_master((u32)periph->reg_addr, dma->comm.br, dma->comm.cpol,
+ spi_disable((uint32_t)periph->reg_addr);
+ spi_init_master((uint32_t)periph->reg_addr, dma->comm.br, dma->comm.cpol,
dma->comm.cpha, dma->comm.dff, dma->comm.lsbfirst);
- spi_enable_software_slave_management((u32)periph->reg_addr);
- spi_set_nss_high((u32)periph->reg_addr);
- spi_enable((u32)periph->reg_addr);
+ spi_enable_software_slave_management((uint32_t)periph->reg_addr);
+ spi_set_nss_high((uint32_t)periph->reg_addr);
+ spi_enable((uint32_t)periph->reg_addr);
}
/*
@@ -554,12 +554,12 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
*/
if (trans->input_length == 0) {
/* run the dummy rx dma for the complete transaction length */
- spi_configure_dma(dma->dma, dma->rx_chan, (u32)dma->spidr,
- (u32)&(dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE);
+ spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr,
+ (uint32_t)&(dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE);
} else {
/* run the real rx dma for input_length */
- spi_configure_dma(dma->dma, dma->rx_chan, (u32)dma->spidr,
- (u32)trans->input_buf, trans->input_length, trans->dss, TRUE);
+ spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr,
+ (uint32_t)trans->input_buf, trans->input_length, trans->dss, TRUE);
/* use dummy rx dma for the rest */
if (trans->output_length > trans->input_length) {
/* Enable use of second dma transfer with dummy buffer (cleared in ISR) */
@@ -581,11 +581,11 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
* the dummy is used right from the start.
*/
if (trans->output_length == 0) {
- spi_configure_dma(dma->dma, dma->tx_chan, (u32)dma->spidr,
- (u32)&(dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE);
+ spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr,
+ (uint32_t)&(dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE);
} else {
- spi_configure_dma(dma->dma, dma->tx_chan, (u32)dma->spidr,
- (u32)trans->output_buf, trans->output_length, trans->dss, TRUE);
+ spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr,
+ (uint32_t)trans->output_buf, trans->output_length, trans->dss, TRUE);
if (trans->input_length > trans->output_length) {
/* Enable use of second dma transfer with dummy buffer (cleared in ISR) */
dma->tx_extra_dummy_dma = TRUE;
@@ -604,8 +604,8 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
dma_enable_channel(dma->dma, dma->tx_chan);
/* Enable SPI transfers via DMA */
- spi_enable_rx_dma((u32)periph->reg_addr);
- spi_enable_tx_dma((u32)periph->reg_addr);
+ spi_enable_rx_dma((uint32_t)periph->reg_addr);
+ spi_enable_tx_dma((uint32_t)periph->reg_addr);
}
@@ -619,7 +619,7 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran
void spi1_arch_init(void) {
// set dma options
- spi1_dma.spidr = (u32)&SPI1_DR;
+ spi1_dma.spidr = (uint32_t)&SPI1_DR;
spi1_dma.dma = DMA1;
spi1_dma.rx_chan = DMA_CHANNEL2;
spi1_dma.tx_chan = DMA_CHANNEL3;
@@ -690,7 +690,7 @@ void spi1_arch_init(void) {
void spi2_arch_init(void) {
// set dma options
- spi2_dma.spidr = (u32)&SPI2_DR;
+ spi2_dma.spidr = (uint32_t)&SPI2_DR;
spi2_dma.dma = DMA1;
spi2_dma.rx_chan = DMA_CHANNEL4;
spi2_dma.tx_chan = DMA_CHANNEL5;
@@ -760,7 +760,7 @@ void spi2_arch_init(void) {
void spi3_arch_init(void) {
// set the default configuration
- spi3_dma.spidr = (u32)&SPI3_DR;
+ spi3_dma.spidr = (uint32_t)&SPI3_DR;
spi3_dma.dma = DMA2;
spi3_dma.rx_chan = DMA_CHANNEL1;
spi3_dma.tx_chan = DMA_CHANNEL2;
@@ -914,7 +914,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) {
dma_disable_transfer_complete_interrupt(dma->dma, dma->rx_chan);
/* Disable SPI Rx request */
- spi_disable_rx_dma((u32)periph->reg_addr);
+ spi_disable_rx_dma((uint32_t)periph->reg_addr);
/* Disable DMA rx channel */
dma_disable_channel(dma->dma, dma->rx_chan);
@@ -931,10 +931,10 @@ void process_rx_dma_interrupt(struct spi_periph *periph) {
dma->rx_extra_dummy_dma = FALSE;
/* Use the difference in length between rx and tx */
- u16 len_remaining = trans->output_length - trans->input_length;
+ uint16_t len_remaining = trans->output_length - trans->input_length;
- spi_configure_dma(dma->dma, dma->rx_chan, (u32)dma->spidr,
- (u32)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE);
+ spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr,
+ (uint32_t)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE);
dma_set_read_from_peripheral(dma->dma, dma->rx_chan);
dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH);
@@ -943,7 +943,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) {
/* Enable DMA channels */
dma_enable_channel(dma->dma, dma->rx_chan);
/* Enable SPI transfers via DMA */
- spi_enable_rx_dma((u32)periph->reg_addr);
+ spi_enable_rx_dma((uint32_t)periph->reg_addr);
}
else {
/*
@@ -976,7 +976,7 @@ void process_tx_dma_interrupt(struct spi_periph *periph) {
dma_disable_transfer_complete_interrupt(dma->dma, dma->tx_chan);
/* Disable SPI TX request */
- spi_disable_tx_dma((u32)periph->reg_addr);
+ spi_disable_tx_dma((uint32_t)periph->reg_addr);
/* Disable DMA tx channel */
dma_disable_channel(dma->dma, dma->tx_chan);
@@ -992,10 +992,10 @@ void process_tx_dma_interrupt(struct spi_periph *periph) {
dma->tx_extra_dummy_dma = FALSE;
/* Use the difference in length between tx and rx */
- u16 len_remaining = trans->input_length - trans->output_length;
+ uint16_t len_remaining = trans->input_length - trans->output_length;
- spi_configure_dma(dma->dma, dma->tx_chan, (u32)dma->spidr,
- (u32)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE);
+ spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr,
+ (uint32_t)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE);
dma_set_read_from_memory(dma->dma, dma->tx_chan);
dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM);
@@ -1004,7 +1004,7 @@ void process_tx_dma_interrupt(struct spi_periph *periph) {
/* Enable DMA channels */
dma_enable_channel(dma->dma, dma->tx_chan);
/* Enable SPI transfers via DMA */
- spi_enable_tx_dma((u32)periph->reg_addr);
+ spi_enable_tx_dma((uint32_t)periph->reg_addr);
}
}
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
index 5e4058ca61..fb73f70983 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
@@ -42,40 +42,40 @@
void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
/* Configure USART */
- usart_set_baudrate((u32)p->reg_addr, baud);
- usart_set_databits((u32)p->reg_addr, 8);
- usart_set_stopbits((u32)p->reg_addr, USART_STOPBITS_1);
- usart_set_parity((u32)p->reg_addr, USART_PARITY_NONE);
+ usart_set_baudrate((uint32_t)p->reg_addr, baud);
+ usart_set_databits((uint32_t)p->reg_addr, 8);
+ usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1);
+ usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE);
/* Disable Idle Line interrupt */
- USART_CR1((u32)p->reg_addr) &= ~USART_CR1_IDLEIE;
+ USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_IDLEIE;
/* Disable LIN break detection interrupt */
- USART_CR2((u32)p->reg_addr) &= ~USART_CR2_LBDIE;
+ USART_CR2((uint32_t)p->reg_addr) &= ~USART_CR2_LBDIE;
/* Enable USART1 Receive interrupts */
- USART_CR1((u32)p->reg_addr) |= USART_CR1_RXNEIE;
+ USART_CR1((uint32_t)p->reg_addr) |= USART_CR1_RXNEIE;
/* Enable the USART */
- usart_enable((u32)p->reg_addr);
+ usart_enable((uint32_t)p->reg_addr);
}
void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) {
- u32 mode = 0;
+ uint32_t mode = 0;
if (tx_enabled)
mode |= USART_MODE_TX;
if (rx_enabled)
mode |= USART_MODE_RX;
/* set mode to Tx, Rx or TxRx */
- usart_set_mode((u32)p->reg_addr, mode);
+ usart_set_mode((uint32_t)p->reg_addr, mode);
if (hw_flow_control) {
- usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_RTS_CTS);
+ usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_RTS_CTS);
}
else {
- usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_NONE);
+ usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_NONE);
}
}
@@ -86,7 +86,7 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) {
if (temp == p->tx_extract_idx)
return; // no room
- USART_CR1((u32)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt
+ USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt
// check if in process of sending data
if (p->tx_running) { // yes, add to queue
@@ -95,61 +95,61 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) {
}
else { // no, set running flag and write to output register
p->tx_running = TRUE;
- usart_send((u32)p->reg_addr, data);
+ usart_send((uint32_t)p->reg_addr, data);
}
- USART_CR1((u32)p->reg_addr) |= USART_CR1_TXEIE; // Enable TX interrupt
+ USART_CR1((uint32_t)p->reg_addr) |= USART_CR1_TXEIE; // Enable TX interrupt
}
static inline void usart_isr(struct uart_periph* p) {
- if (((USART_CR1((u32)p->reg_addr) & USART_CR1_TXEIE) != 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_TXE) != 0)) {
+ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_TXEIE) != 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_TXE) != 0)) {
// check if more data to send
if (p->tx_insert_idx != p->tx_extract_idx) {
- usart_send((u32)p->reg_addr,p->tx_buf[p->tx_extract_idx]);
+ usart_send((uint32_t)p->reg_addr,p->tx_buf[p->tx_extract_idx]);
p->tx_extract_idx++;
p->tx_extract_idx %= UART_TX_BUFFER_SIZE;
}
else {
p->tx_running = FALSE; // clear running flag
- USART_CR1((u32)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt
+ USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt
}
}
- if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_RXNE) != 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_ORE) == 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_NE) == 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_FE) == 0)) {
+ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_RXNE) != 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) == 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_NE) == 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_FE) == 0)) {
uint16_t temp = (p->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;;
- p->rx_buf[p->rx_insert_idx] = usart_recv((u32)p->reg_addr);
+ p->rx_buf[p->rx_insert_idx] = usart_recv((uint32_t)p->reg_addr);
// check for more room in queue
if (temp != p->rx_extract_idx)
p->rx_insert_idx = temp; // update insert index
}
else {
/* ORE, NE or FE error - read USART_DR reg and log the error */
- if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_ORE) != 0)) {
- usart_recv((u32)p->reg_addr);
+ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) != 0)) {
+ usart_recv((uint32_t)p->reg_addr);
p->ore++;
}
- if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_NE) != 0)) {
- usart_recv((u32)p->reg_addr);
+ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_NE) != 0)) {
+ usart_recv((uint32_t)p->reg_addr);
p->ne_err++;
}
- if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
- ((USART_SR((u32)p->reg_addr) & USART_SR_FE) != 0)) {
- usart_recv((u32)p->reg_addr);
+ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
+ ((USART_SR((uint32_t)p->reg_addr) & USART_SR_FE) != 0)) {
+ usart_recv((uint32_t)p->reg_addr);
p->fe_err++;
}
}
}
-static inline void usart_enable_irq(u8 IRQn) {
+static inline void usart_enable_irq(uint8_t IRQn) {
/* Note:
* In libstm32 times the priority of this interrupt was set to
* preemption priority 2 and sub priority 1
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index a39b5c7e39..4a56067c5a 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -65,7 +65,7 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
/** Set PWM channel configuration
*/
-static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral,
+static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral,
enum tim_oc_id oc_id) {
timer_disable_oc_output(timer_peripheral, oc_id);
@@ -80,13 +80,13 @@ static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral,
/** Set GPIO configuration
*/
#if defined(STM32F4)
-static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 af_num, u32 en) {
+static inline void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, uint32_t en) {
rcc_peripheral_enable_clock(&RCC_AHB1ENR, en);
gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin);
gpio_set_af(gpioport, af_num, pin);
}
#elif defined(STM32F1)
-static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 none, u32 en) {
+static inline void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none, uint32_t en) {
rcc_peripheral_enable_clock(&RCC_APB2ENR, en | RCC_APB2ENR_AFIOEN);
gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin);
@@ -95,7 +95,7 @@ static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 none, u32 en) {
/** Set Timer configuration
*/
-static inline void set_servo_timer(u32 timer, u32 period, u8 channels_mask) {
+static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask) {
timer_reset(timer);
/* Timer global mode:
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
index b979a4e2d3..db20493eda 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
@@ -52,6 +52,20 @@ uint32_t ppm_last_pulse_time;
bool_t ppm_data_valid;
static uint32_t timer_rollover_cnt;
+/*
+ * Timer clock frequency (before prescaling) is twice the frequency
+ * of the APB domain to which the timer is connected.
+ */
+
+#ifdef STM32F1
+/**
+ * HCLK = 72MHz, Timer clock also 72MHz since
+ * TIM1_CLK = APB2 = 72MHz
+ * TIM2_CLK = 2 * APB1 = 2 * 32MHz
+ */
+#define PPM_TIMER_CLK AHB_CLK
+#endif
+
#if USE_PPM_TIM2
PRINT_CONFIG_MSG("Using TIM2 for PPM input.")
@@ -60,6 +74,14 @@ PRINT_CONFIG_MSG("Using TIM2 for PPM input.")
#define PPM_PERIPHERAL RCC_APB1ENR_TIM2EN
#define PPM_TIMER TIM2
+#ifdef STM32F4
+/* Since APB prescaler != 1 :
+ * Timer clock frequency (before prescaling) is twice the frequency
+ * of the APB domain to which the timer is connected.
+ */
+#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2)
+#endif
+
#elif USE_PPM_TIM1
PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
@@ -68,6 +90,12 @@ PRINT_CONFIG_MSG("Using TIM1 for PPM input.")
#define PPM_PERIPHERAL RCC_APB2ENR_TIM1EN
#define PPM_TIMER TIM1
+#ifdef STM32F4
+#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2)
+#endif
+
+#else
+#error Unknown PPM input timer configuration.
#endif
void ppm_arch_init ( void ) {
@@ -86,7 +114,7 @@ void ppm_arch_init ( void ) {
timer_set_mode(PPM_TIMER, TIM_CR1_CKD_CK_INT,
TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
timer_set_period(PPM_TIMER, 0xFFFF);
- timer_set_prescaler(PPM_TIMER, (AHB_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1);
+ timer_set_prescaler(PPM_TIMER, (PPM_TIMER_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1);
/* TIM configuration: Input Capture mode ---------------------
The Rising edge is used as active edge
@@ -96,7 +124,7 @@ void ppm_arch_init ( void ) {
#elif defined PPM_PULSE_TYPE && PPM_PULSE_TYPE == PPM_PULSE_TYPE_NEGATIVE
timer_ic_set_polarity(PPM_TIMER, PPM_CHANNEL, TIM_IC_FALLING);
#else
-#error "Unknown PM_PULSE_TYPE"
+#error "Unknown PPM_PULSE_TYPE"
#endif
timer_ic_set_input(PPM_TIMER, PPM_CHANNEL, PPM_TIMER_INPUT);
timer_ic_set_prescaler(PPM_TIMER, PPM_CHANNEL, TIM_IC_PSC_OFF);
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h
index fbe1ece8da..4d866004f1 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h
@@ -33,10 +33,17 @@
#include "mcu_periph/sys_time.h"
/**
- * The ppm counter is running at cpu freq / 72 or 168 / 8
- * so the counter has 1/8 us resolution
+ * The ppm counter is set-up to have 1/6 us resolution.
+ *
+ * The timer clock frequency (before prescaling):
+ * STM32F1:
+ * TIM1 -> APB2 = HCLK = 72MHz
+ * TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz
+ * STM32F4:
+ * TIM1 -> 2 * APB2 = 2 * 84MHz = 168MHz
+ * TIM2 -> 2 * APB1 = 2 * 42MHz = 84MHz
*/
-#define RC_PPM_TICKS_PER_USEC 8
+#define RC_PPM_TICKS_PER_USEC 6
#define RC_PPM_TICKS_OF_USEC(_v) ((_v)*RC_PPM_TICKS_PER_USEC)
#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (int32_t)((_v)*RC_PPM_TICKS_PER_USEC)
diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c
index 411e97552d..3b814a5df3 100644
--- a/sw/airborne/arch/stm32/subsystems/settings_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c
@@ -35,12 +35,7 @@
#include "subsystems/settings.h"
-#if defined(STM32F1)
-#include
-#elif defined(STM32F4)
-#include
-#endif
-
+#include
#include
#include
diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h
index 08605e9843..431030eb31 100644
--- a/sw/airborne/boards/ardrone/at_com.h
+++ b/sw/airborne/boards/ardrone/at_com.h
@@ -29,6 +29,8 @@
#ifndef BOARDS_ARDRONE_AT_COM_H
#define BOARDS_ARDRONE_AT_COM_H
+#define NAVDATA_HEADER (0x55667788)
+
//Define the AT_REF bits
typedef enum {
REF_TAKEOFF = 1U << 9,
@@ -149,6 +151,55 @@ typedef struct _navdata_phys_measures_t {
uint32_t vrefIDG; // ref volt IDG gyro [LSB]
} __attribute__ ((packed)) navdata_phys_measures_t;
+//Navdata gps packet
+typedef double float64_t; //TODO: Fix this nicely, but this is only used here
+typedef float float32_t; //TODO: Fix this nicely, but this is only used here
+typedef struct _navdata_gps_t {
+ uint16_t tag; /*!< Navdata block ('option') identifier */
+ uint16_t size; /*!< set this to the size of this structure */
+ float64_t lat; /*!< Latitude */
+ float64_t lon; /*!< Longitude */
+ float64_t elevation; /*!< Elevation */
+ float64_t hdop; /*!< hdop */
+ int32_t data_available; /*!< When there is data available */
+ uint8_t unk_0[8];
+ float64_t lat0; /*!< Latitude ??? */
+ float64_t lon0; /*!< Longitude ??? */
+ float64_t lat_fuse; /*!< Latitude fused */
+ float64_t lon_fuse; /*!< Longitude fused */
+ uint32_t gps_state; /*!< State of the GPS, still need to figure out */
+ uint8_t unk_1[40];
+ float64_t vdop; /*!< vdop */
+ float64_t pdop; /*!< pdop */
+ float32_t speed; /*!< speed */
+ uint32_t last_frame_timestamp; /*!< Timestamp from the last frame */
+ float32_t degree; /*!< Degree */
+ float32_t degree_mag; /*!< Degree of the magnetic */
+ uint8_t unk_2[16];
+ struct{
+ uint8_t sat;
+ uint8_t unk;
+ }channels[12];
+ int32_t gps_plugged; /*!< When the gps is plugged */
+ uint8_t unk_3[108];
+ float64_t gps_time; /*!< The gps time of week */
+ uint16_t week; /*!< The gps week */
+ uint8_t gps_fix; /*!< The gps fix */
+ uint8_t num_sattelites; /*!< Number of sattelites */
+ uint8_t unk_4[24];
+ float64_t ned_vel_c0; /*!< NED velocity */
+ float64_t ned_vel_c1; /*!< NED velocity */
+ float64_t ned_vel_c2; /*!< NED velocity */
+ float64_t pos_accur_c0; /*!< Position accuracy */
+ float64_t pos_accur_c1; /*!< Position accuracy */
+ float64_t pos_accur_c2; /*!< Position accuracy */
+ float32_t speed_acur; /*!< Speed accuracy */
+ float32_t time_acur; /*!< Time accuracy */
+ uint8_t unk_5[72];
+ float32_t temprature;
+ float32_t pressure;
+} __attribute__ ((packed)) navdata_gps_t;
+
//External functions
extern void init_at_com(void);
extern void at_com_recieve_navdata(unsigned char* buffer);
diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h
index c325f3ec40..01bb97452d 100644
--- a/sw/airborne/boards/ardrone2_sdk.h
+++ b/sw/airborne/boards/ardrone2_sdk.h
@@ -6,7 +6,7 @@
/* Internal communication */
#define ARDRONE_NAVDATA_PORT 5554
#define ARDRONE_AT_PORT 5556
-#define ARDRONE_NAVDATA_BUFFER_SIZE 2048
+#define ARDRONE_NAVDATA_BUFFER_SIZE 4096
#define ARDRONE_IP "192.168.1.1"
/* Default actuators driver */
diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h
index 6e6899bbce..d4fb6deab1 100644
--- a/sw/airborne/boards/krooz_sd.h
+++ b/sw/airborne/boards/krooz_sd.h
@@ -3,7 +3,7 @@
#define BOARD_KROOZ
-/* Krooz/M has a 12MHz external clock and 168MHz internal. */
+/* KroozSD has a 12MHz external clock and 168MHz internal. */
#define EXT_CLK 12000000
#define AHB_CLK 168000000
@@ -210,7 +210,7 @@
#define BOARD_HAS_BARO 1
/* PWM */
-#define PWM_USE_TIM2 1
+//#define PWM_USE_TIM2 1
#define PWM_USE_TIM3 1
#define PWM_USE_TIM4 1
#define PWM_USE_TIM5 1
@@ -383,11 +383,11 @@
#define USE_PPM_TIM2 1
#define PPM_CHANNEL TIM_IC2
-#define PPM_TIMER_INPUT TIM_IC_IN_TI1
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
#define PPM_IRQ NVIC_TIM2_IRQ
//#define PPM_IRQ2 NVIC_TIM2_UP_TIM10_IRQ
// Capture/Compare InteruptEnable and InterruptFlag
-#define PPM_CC_EN TIM_DIER_CC2IE
+#define PPM_CC_IE TIM_DIER_CC2IE
#define PPM_CC_IF TIM_SR_CC2IF
#define PPM_GPIO_PORT GPIOB
#define PPM_GPIO_PIN GPIO3
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index 28e199cbe0..f18b7d34eb 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -59,7 +59,7 @@
*/
#define UART1_GPIO_AF 0
#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX
-#define UART1_GPIO_RX GPIO_BANK_USART1_RX
+#define UART1_GPIO_RX GPIO_USART1_RX
#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX
#define UART1_GPIO_TX GPIO_USART1_TX
@@ -129,6 +129,7 @@
#define PPM_CC_IF TIM_SR_CC2IF
#define PPM_GPIO_PORT GPIOA
#define PPM_GPIO_PIN GPIO1
+#define PPM_GPIO_AF 0
/* ADC */
diff --git a/sw/airborne/firmwares/beth/overo_sfb_controller.c b/sw/airborne/firmwares/beth/overo_sfb_controller.c
index e2ef819db0..66730bbbc9 100644
--- a/sw/airborne/firmwares/beth/overo_sfb_controller.c
+++ b/sw/airborne/firmwares/beth/overo_sfb_controller.c
@@ -187,4 +187,4 @@ void calc_sfb_cmd(void) {
//if (_CO.cmd_thrust<0.) _CO.cmd_thrust = 0;
Bound(_CO.cmd_sfb_thrust,0,100);
Bound(_CO.cmd_sfb_pitch,-100,100);
-}
\ No newline at end of file
+}
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 760e90aefe..6b0334ab12 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -253,7 +253,7 @@ void event_task_fbw( void) {
trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
#endif
#ifdef COMMAND_YAW
- trimmed_commands[COMMAND_YAW] = ChopAbs(command_yaw_trim, MAX_PPRZ);
+ trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
#endif
SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index d4a4e02c70..2b816e0625 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -78,6 +78,11 @@ static inline int ahrs_is_aligned(void) {
#include "autopilot_arming_yaw.h"
#endif
+#ifndef MODE_STARTUP
+#define MODE_STARTUP AP_MODE_KILL
+PRINT_CONFIG_MSG("Using AP_MODE_KILL as MODE_STARTUP")
+#endif
+
static void send_alive(void) {
DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
}
@@ -168,7 +173,7 @@ static void send_baro_raw(void) {
}
void autopilot_init(void) {
- autopilot_mode = AP_MODE_KILL;
+ autopilot_mode = MODE_STARTUP;
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE;
@@ -200,6 +205,36 @@ void autopilot_init(void) {
}
+static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) {
+ if (autopilot_in_flight) {
+ if (autopilot_in_flight_counter > 0) {
+ if (stabilization_cmd[COMMAND_THRUST] == 0) {
+ autopilot_in_flight_counter--;
+ if (autopilot_in_flight_counter == 0) {
+ autopilot_in_flight = FALSE;
+ }
+ }
+ else { /* !THROTTLE_STICK_DOWN */
+ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
+ }
+ }
+ }
+ else { /* not in flight */
+ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
+ motors_on) {
+ if (stabilization_cmd[COMMAND_THRUST] > 0) {
+ autopilot_in_flight_counter++;
+ if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
+ autopilot_in_flight = TRUE;
+ }
+ else { /* THROTTLE_STICK_DOWN */
+ autopilot_in_flight_counter = 0;
+ }
+ }
+ }
+}
+
+
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
@@ -226,6 +261,10 @@ INFO("Using FAILSAFE_GROUND_DETECT")
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
+ // when we dont have RC, check in flight by looking at throttle
+ if (radio_control.status != RC_OK) {
+ autopilot_check_in_flight_no_rc(autopilot_motors_on);
+ }
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
index 09777e371b..7a0572f69e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -19,7 +19,7 @@
* Boston, MA 02111-1307, USA.
*/
-/** @file firmwares/rotorcraft/stabilization_attitude.h
+/** @file firmwares/rotorcraft/stabilization/stabilization_attitude.h
* General attitude stabilization interface for rotorcrafts.
* The actual implementation is automatically included.
*/
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
index 8e08d952b9..e3deb37a7a 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -24,6 +24,7 @@
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "paparazzi.h"
#include "generated/airframe.h"
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
index b7b216771e..7054d1824b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
@@ -84,16 +84,16 @@ float stabilization_attitude_get_heading_f(void) {
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
- const int32_t max_phi_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ;
- const int32_t max_theta_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ;
- const int32_t max_r_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ;
+ const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
+ const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
+ const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
- sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * max_phi_scale);
- sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * max_theta_scale);
+ sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
+ sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
- sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ);
+ sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (autopilot_mode == AP_MODE_FORWARD) {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
index 0bb9aa4be4..78fc43f804 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
@@ -196,22 +196,22 @@ void stabilization_rate_read_rc( void ) {
//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
void stabilization_rate_read_rc_switched_sticks( void ) {
-
+
if(ROLL_RATE_DEADBAND_EXCEEDED())
stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
else
stabilization_rate_sp.r = 0;
-
+
if(PITCH_RATE_DEADBAND_EXCEEDED())
stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
else
stabilization_rate_sp.q = 0;
-
+
if(YAW_RATE_DEADBAND_EXCEEDED())
stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
else
stabilization_rate_sp.p = 0;
-
+
// Setpoint at ref resolution
INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC);
}
diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c
index 57991a715b..428472080c 100644
--- a/sw/airborne/math/pprz_geodetic_int.c
+++ b/sw/airborne/math/pprz_geodetic_int.c
@@ -142,9 +142,34 @@ void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct Ecef
ENU_OF_TO_NED(*ned, enu);
}
-/* check if resolution of INT32_TRIG_FRAC (14) is enough here */
+
+void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
+
+ const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0] * enu->x +
+ (int64_t)def->ltp_of_ecef.m[3] * enu->y +
+ (int64_t)def->ltp_of_ecef.m[6] * enu->z;
+ ecef->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
+
+ const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[1] * enu->x +
+ (int64_t)def->ltp_of_ecef.m[4] * enu->y +
+ (int64_t)def->ltp_of_ecef.m[7] * enu->z;
+ ecef->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
+
+ /* first element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ENU_to_ECEF */
+ const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[5] * enu->y +
+ (int64_t)def->ltp_of_ecef.m[8] * enu->z;
+ ecef->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
+
+}
+
+void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
+ struct EnuCoor_i enu;
+ ENU_OF_TO_NED(enu, *ned);
+ ecef_of_enu_vect_i(ecef, def, &enu);
+}
+
void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
- INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
+ ecef_of_enu_vect_i(ecef, def, enu);
INT32_VECT3_ADD(*ecef, def->ecef);
}
@@ -154,16 +179,6 @@ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct N
ecef_of_enu_point_i(ecef, def, &enu);
}
-void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
- INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
-}
-
-void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
- struct EnuCoor_i enu;
- ENU_OF_TO_NED(enu, *ned);
- ecef_of_enu_vect_i(ecef, def, &enu);
-}
-
void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
struct EcefCoor_i ecef;
diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c
index f677b603bf..18c03c446b 100644
--- a/sw/airborne/modules/ins/ins_xsens700.c
+++ b/sw/airborne/modules/ins/ins_xsens700.c
@@ -22,8 +22,9 @@
*
*/
-/** \file xsens.c
- * \brief Parser for the Xsens protocol
+/**
+ * @file modules/ins/ins_xsens700.c
+ * Parser for the Xsens protocol.
*/
#include "ins_module.h"
diff --git a/sw/airborne/modules/loggers/high_speed_logger_spi_link.c b/sw/airborne/modules/loggers/high_speed_logger_spi_link.c
index fb4e4ed676..81c2e734f9 100644
--- a/sw/airborne/modules/loggers/high_speed_logger_spi_link.c
+++ b/sw/airborne/modules/loggers/high_speed_logger_spi_link.c
@@ -62,8 +62,8 @@ void high_speed_logger_spi_link_periodic(void)
high_speed_logger_spi_link_data.acc_y = imu.accel_unscaled.y;
high_speed_logger_spi_link_data.acc_z = imu.accel_unscaled.z;
high_speed_logger_spi_link_data.mag_x = imu.mag_unscaled.x;
- high_speed_logger_spi_link_data.mag_y = imu.mag_unscaled.x;
- high_speed_logger_spi_link_data.mag_z = imu.mag_unscaled.x;
+ high_speed_logger_spi_link_data.mag_y = imu.mag_unscaled.y;
+ high_speed_logger_spi_link_data.mag_z = imu.mag_unscaled.z;
spi_submit(&(HIGH_SPEED_LOGGER_SPI_LINK_DEVICE), &high_speed_logger_spi_link_transaction);
}
diff --git a/sw/airborne/modules/meteo/humid_pcap01.c b/sw/airborne/modules/meteo/humid_pcap01.c
index 3a0e8617ee..e38e5de2a9 100644
--- a/sw/airborne/modules/meteo/humid_pcap01.c
+++ b/sw/airborne/modules/meteo/humid_pcap01.c
@@ -195,14 +195,6 @@ void pcap01readRegister(uint8_t reg)
*
* function where current measurement data from pcap01 is read into
* global sensor variable
-*
-* \param control Control command
-* possible commands:
-* PCAP01_PU_RESET : Hard reset of the device
-* PCAP01_IN_RESET : Software reset
-* PCAP01_START : Start measurement
-* PCAP01_START : Start measurement
-* PCAP01_TERM : Stop measurement
*/
void pcap01_periodic(void)
{
diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c
index 84d6243311..9ee17dc80f 100644
--- a/sw/airborne/modules/sensors/airspeed_amsys.c
+++ b/sw/airborne/modules/sensors/airspeed_amsys.c
@@ -42,16 +42,16 @@
#ifndef AIRSPEED_AMSYS_SCALE
#define AIRSPEED_AMSYS_SCALE 1
#endif
-#ifndef AIRSPEED_AMSYS_OFFSET
-#define AIRSPEED_AMSYS_OFFSET 0
-#endif
#define AIRSPEED_AMSYS_OFFSET_MAX 29491
#define AIRSPEED_AMSYS_OFFSET_MIN 3277
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT 40
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG 60
#define AIRSPEED_AMSYS_NBSAMPLES_AVRG 10
#ifndef AIRSPEED_AMSYS_MAXPRESURE
-#define AIRSPEED_AMSYS_MAXPRESURE 2068//2073 //Pascal
+#define AIRSPEED_AMSYS_MAXPRESURE 2068 //003-2068, 001-689 //Pascal
+#endif
+#ifndef AIRSPEED_AMSYS_FILTER
+#define AIRSPEED_AMSYS_FILTER 0
#endif
#ifndef AIRSPEED_AMSYS_I2C_DEV
#define AIRSPEED_AMSYS_I2C_DEV i2c0
@@ -68,8 +68,9 @@
uint16_t airspeed_amsys_raw;
uint16_t tempAS_amsys_raw;
bool_t airspeed_amsys_valid;
-float airspeed_tmp;
-float pressure_amsys; //Pascal
+float airspeed_amsys_offset;
+float airspeed_amsys_tmp;
+float airspeed_amsys_p; //Pascal
float airspeed_amsys; //mps
float airspeed_scale;
float airspeed_filter;
@@ -79,17 +80,23 @@ struct i2c_transaction airspeed_amsys_i2c_trans;
volatile bool_t airspeed_amsys_i2c_done;
float airspeed_temperature = 0.0;
float airspeed_old = 0.0;
-
+bool_t airspeed_amsys_offset_init;
+double airspeed_amsys_offset_tmp;
+uint16_t airspeed_amsys_cnt;
void airspeed_amsys_init( void ) {
airspeed_amsys_raw = 0;
airspeed_amsys = 0.0;
- pressure_amsys = 0.0;
+ airspeed_amsys_p = 0.0;
+ airspeed_amsys_offset = 0;
+ airspeed_amsys_offset_tmp = 0;
airspeed_amsys_i2c_done = TRUE;
airspeed_amsys_valid = TRUE;
- airspeed_scale = AIRSPEED_SCALE;
- airspeed_filter = AIRSPEED_FILTER;
+ airspeed_amsys_offset_init = FALSE;
+ airspeed_scale = AIRSPEED_AMSYS_SCALE;
+ airspeed_filter = AIRSPEED_AMSYS_FILTER;
airspeed_amsys_i2c_trans.status = I2CTransDone;
+ airspeed_amsys_cnt = AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT + AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG;
}
void airspeed_amsys_read_periodic( void ) {
@@ -101,10 +108,21 @@ void airspeed_amsys_read_periodic( void ) {
i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4);
#endif
+#if USE_AIRSPEED
+ stateSetAirspeed_f(&airspeed_amsys);
+#endif
+
#else // SITL
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
+
+
+#ifdef AIRSPEED_AMSYS_SYNC_SEND
+ DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &airspeed_amsys_p, &airspeed_amsys_tmp, &airspeed_amsys, &airspeed_amsys_offset);
+#else
+ RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &airspeed_amsys_p, &airspeed_amsys_tmp, &airspeed_amsys, &airspeed_temperature));
+#endif
}
void airspeed_amsys_read_event( void ) {
@@ -134,23 +152,38 @@ void airspeed_amsys_read_event( void ) {
airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
// calculate raw to pressure
- pressure_amsys = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN);
+ airspeed_amsys_p = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN);
+ if (!airspeed_amsys_offset_init) {
+ --airspeed_amsys_cnt;
+ // Check if averaging completed
+ if (airspeed_amsys_cnt == 0) {
+ // Calculate average
+ airspeed_amsys_offset = (float)(airspeed_amsys_offset_tmp / AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG);
+ airspeed_amsys_offset_init = TRUE;
+ }
+ // Check if averaging needs to continue
+ else if (airspeed_amsys_cnt <= AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG)
+ airspeed_amsys_offset_tmp += airspeed_amsys_p;
+
+ airspeed_amsys = 0.;
+
+ }
+ else {
+ airspeed_amsys_p = airspeed_amsys_p - airspeed_amsys_offset;
+ if (airspeed_amsys_p <= 0) airspeed_amsys_p=0.000000001;
+ airspeed_amsys_tmp = sqrtf(2*(airspeed_amsys_p)*airspeed_scale/1.2041); // convert pressure to airspeed
+ // Lowpassfiltering
+ airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_amsys_tmp;
+ airspeed_old = airspeed_amsys;
+ //New value available
+ }
+
+ } /*else {
+ airspeed_amsys = 0.0;
+ }*/
- airspeed_tmp = sqrtf(2*(pressure_amsys)*airspeed_scale/1.2041); //without offset
- // Lowpass filter
- airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_tmp;
- airspeed_old = airspeed_amsys;
-#if USE_AIRSPEED
- stateSetAirspeed_f(&airspeed_amsys);
-#endif
-#ifdef SENSOR_SYNC_SEND
- DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature);
-#else
- RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature));
-#endif
- }
// Transaction has been read
airspeed_amsys_i2c_trans.status = I2CTransDone;
diff --git a/sw/airborne/modules/sensors/airspeed_otf.c b/sw/airborne/modules/sensors/airspeed_otf.c
index 4b5f5bf7bc..9455d68949 100644
--- a/sw/airborne/modules/sensors/airspeed_otf.c
+++ b/sw/airborne/modules/sensors/airspeed_otf.c
@@ -20,8 +20,9 @@
*
*/
-/** \file met_ap_otf.c
- * \brief UART interface for Aeroprobe On-The-Fly! air data computer
+/**
+ * @file modules/sensors/airspeed_otf.c
+ * UART interface for Aeroprobe On-The-Fly! air data computer.
*
*/
diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h
index 7844695db6..98f028582f 100644
--- a/sw/airborne/modules/sensors/baro_amsys.h
+++ b/sw/airborne/modules/sensors/baro_amsys.h
@@ -33,7 +33,6 @@
extern uint16_t baro_amsys_adc;
// extern float baro_amsys_offset;
extern bool_t baro_amsys_valid;
-// extern bool_t baro_amsys_updated;
// extern bool_t baro_amsys_enabled;
extern float baro_amsys_altitude;
// extern float baro_amsys_r;
diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h
index ed6b7d0fe3..c492ef8fc4 100644
--- a/sw/airborne/modules/sensors/baro_ets.h
+++ b/sw/airborne/modules/sensors/baro_ets.h
@@ -50,7 +50,6 @@
extern uint16_t baro_ets_adc;
extern uint16_t baro_ets_offset;
extern bool_t baro_ets_valid;
-extern bool_t baro_ets_updated;
extern bool_t baro_ets_enabled;
extern float baro_ets_altitude;
extern float baro_ets_r;
diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c
index 8daba2dca4..dbe127b4f8 100644
--- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c
+++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c
@@ -20,8 +20,9 @@
*
*/
-/** \file baro_ms5611_i2c.c
- * \brief Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
+/**
+ * @file modules/sensors/baro_ms5611_i2c.c
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
*
*/
diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c
index 34598ac5c9..ea926b0125 100644
--- a/sw/airborne/peripherals/mpu60x0.c
+++ b/sw/airborne/peripherals/mpu60x0.c
@@ -31,18 +31,38 @@
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
{
+ c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
- c->i2c_bypass = TRUE;
c->drdy_int_enable = FALSE;
- c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
+
+ /* Number of bytes to read starting with MPU60X0_REG_INT_STATUS
+ * By default read only gyro and accel data -> 15 bytes.
+ * Increase to include slave data.
+ */
+ c->nb_bytes = 15;
+ c->nb_slaves = 0;
+
+ c->i2c_bypass = FALSE;
}
void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
{
switch (config->init_status) {
+ case MPU60X0_CONF_RESET:
+ /* device reset, set register values to defaults */
+ mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, (1<<6));
+ config->init_status++;
+ break;
+ case MPU60X0_CONF_USER_RESET:
+ /* trigger FIFO, I2C_MST and SIG_COND resets */
+ mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_FIFO_RESET) |
+ (1 << MPU60X0_I2C_MST_RESET) |
+ (1 << MPU60X0_SIG_COND_RESET)));
+ config->init_status++;
+ break;
case MPU60X0_CONF_PWR:
/* switch to gyroX clock by default */
mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6)));
@@ -68,9 +88,15 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Conf
mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
config->init_status++;
break;
- case MPU60X0_CONF_I2C_BYPASS:
- mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
- config->init_status++;
+ case MPU60X0_CONF_I2C_SLAVES:
+ /* if any, set MPU for I2C slaves and configure them*/
+ if (config->nb_slaves > 0) {
+ /* returns TRUE when all slaves are configured */
+ if (mpu60x0_configure_i2c_slaves(mpu_set, mpu))
+ config->init_status++;
+ }
+ else
+ config->init_status++;
break;
case MPU60X0_CONF_INT_ENABLE:
/* configure data ready interrupt */
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
index 60334d8bd2..00df171767 100644
--- a/sw/airborne/peripherals/mpu60x0.h
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -48,34 +48,62 @@
enum Mpu60x0ConfStatus {
MPU60X0_CONF_UNINIT,
+ MPU60X0_CONF_RESET,
+ MPU60X0_CONF_USER_RESET,
MPU60X0_CONF_PWR,
MPU60X0_CONF_SD,
MPU60X0_CONF_DLPF,
MPU60X0_CONF_GYRO,
MPU60X0_CONF_ACCEL,
- MPU60X0_CONF_I2C_BYPASS,
+ MPU60X0_CONF_I2C_SLAVES,
MPU60X0_CONF_INT_ENABLE,
MPU60X0_CONF_DONE
};
+/// Configuration function prototype
+typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
+
+/// function prototype for configuration of a single I2C slave
+typedef bool_t (*Mpu60x0I2cSlaveConfigure)(Mpu60x0ConfigSet mpu_set, void* mpu);
+
+struct Mpu60x0I2cSlave {
+ Mpu60x0I2cSlaveConfigure configure;
+};
+
struct Mpu60x0Config {
uint8_t smplrt_div; ///< Sample rate divider
enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
enum Mpu60x0GyroRanges gyro_range; ///< deg/s Range
enum Mpu60x0AccelRanges accel_range; ///< g Range
- bool_t i2c_bypass; ///< bypass mpu i2c
bool_t drdy_int_enable; ///< Enable Data Ready Interrupt
uint8_t clk_sel; ///< Clock select
+ uint8_t nb_bytes; ///< number of bytes to read starting with MPU60X0_REG_INT_STATUS
enum Mpu60x0ConfStatus init_status; ///< init status
bool_t initialized; ///< config done flag
+
+ /** Bypass MPU I2C.
+ * Only effective if using the I2C implementation.
+ */
+ bool_t i2c_bypass;
+
+ uint8_t nb_slaves; ///< number of used I2C slaves
+ struct Mpu60x0I2cSlave slaves[5]; ///< I2C slaves
+ enum Mpu60x0MstClk i2c_mst_clk; ///< MPU I2C master clock speed
+ uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate
};
extern void mpu60x0_set_default_config(struct Mpu60x0Config *c);
-/// Configuration function prototype
-typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
-
/// Configuration sequence called once before normal use
extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config);
+/**
+ * Configure I2C slaves of the MPU.
+ * This is I2C/SPI implementation specific.
+ * @param mpu_set configuration function
+ * @param mpu Mpu60x0Spi or Mpu60x0I2c peripheral
+ * @return TRUE when all slaves are configured
+ */
+extern bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu);
+
#endif // MPU60X0_H
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c
index ada9ee4b55..2c79d814a3 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.c
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -44,6 +44,8 @@ void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t
mpu->data_available = FALSE;
mpu->config.initialized = FALSE;
mpu->config.init_status = MPU60X0_CONF_UNINIT;
+
+ mpu->slave_init_status = MPU60X0_I2C_CONF_UNINIT;
}
@@ -70,7 +72,7 @@ void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
/* set read bit and multiple byte bit, then address */
mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS;
- i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, 15);
+ i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
}
}
@@ -84,14 +86,19 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
}
else if (mpu->i2c_trans.status == I2CTransSuccess) {
// Successfull reading
- if (bit_is_set(mpu->i2c_trans.buf[0],0)) {
+ if (bit_is_set(mpu->i2c_trans.buf[0], 0)) {
// new data
- mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf,1);
- mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf,3);
- mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf,5);
- mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf,9);
- mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf,11);
- mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf,13);
+ mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1);
+ mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3);
+ mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5);
+ mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9);
+ mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11);
+ mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13);
+
+ // if we are reading slaves through the mpu, copy the ext_sens_data
+ if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0))
+ memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15);
+
mpu->data_available = TRUE;
}
mpu->i2c_trans.status = I2CTransDone;
@@ -103,12 +110,75 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
mpu->config.init_status--; // Retry config (TODO max retry)
case I2CTransSuccess:
case I2CTransDone:
- mpu->i2c_trans.status = I2CTransDone;
mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config));
- if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone;
+ if (mpu->config.initialized)
+ mpu->i2c_trans.status = I2CTransDone;
break;
default:
break;
}
}
}
+
+/** @todo: only one slave so far. */
+bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu)
+{
+ struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu);
+
+ if (mpu_i2c->slave_init_status == MPU60X0_I2C_CONF_UNINIT)
+ mpu_i2c->slave_init_status++;
+
+ switch (mpu_i2c->slave_init_status) {
+ case MPU60X0_I2C_CONF_I2C_MST_DIS:
+ mpu_set(mpu, MPU60X0_REG_USER_CTRL, 0);
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_I2C_BYPASS_EN:
+ /* switch to I2C passthrough */
+ mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (1<<1));
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_SLAVES_CONFIGURE:
+ /* configure each slave. TODO: currently only one */
+ if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu))
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_I2C_BYPASS_DIS:
+ if (mpu_i2c->config.i2c_bypass) {
+ /* if bypassing I2C skip MPU I2C master setup */
+ mpu_i2c->slave_init_status = MPU60X0_I2C_CONF_DONE;
+ }
+ else {
+ /* disable I2C passthrough again */
+ mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (0<<1));
+ mpu_i2c->slave_init_status++;
+ }
+ break;
+ case MPU60X0_I2C_CONF_I2C_MST_CLK:
+ /* configure MPU I2C master clock and stop/start between slave reads */
+ mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL,
+ ((1<<4) | mpu_i2c->config.i2c_mst_clk));
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_I2C_MST_DELAY:
+ /* Set I2C slaves delayed sample rate */
+ mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay);
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_I2C_SMPLRT:
+ /* I2C slave0 sample rate/2 = 100/2 = 50Hz */
+ mpu_set(mpu, MPU60X0_REG_I2C_SLV4_CTRL, 0);
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_I2C_MST_EN:
+ /* enable internal I2C master */
+ mpu_set(mpu, MPU60X0_REG_USER_CTRL, (1 << MPU60X0_I2C_MST_EN));
+ mpu_i2c->slave_init_status++;
+ break;
+ case MPU60X0_I2C_CONF_DONE:
+ return TRUE;
+ default:
+ break;
+ }
+ return FALSE;
+}
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.h b/sw/airborne/peripherals/mpu60x0_i2c.h
index f7d95eff61..581ad60b45 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.h
+++ b/sw/airborne/peripherals/mpu60x0_i2c.h
@@ -36,6 +36,21 @@
#include "peripherals/mpu60x0.h"
+#define MPU60X0_BUFFER_EXT_LEN 16
+
+enum Mpu60x0I2cSlaveInitStatus {
+ MPU60X0_I2C_CONF_UNINIT,
+ MPU60X0_I2C_CONF_I2C_MST_DIS,
+ MPU60X0_I2C_CONF_I2C_BYPASS_EN,
+ MPU60X0_I2C_CONF_SLAVES_CONFIGURE,
+ MPU60X0_I2C_CONF_I2C_BYPASS_DIS,
+ MPU60X0_I2C_CONF_I2C_MST_CLK,
+ MPU60X0_I2C_CONF_I2C_MST_DELAY,
+ MPU60X0_I2C_CONF_I2C_SMPLRT,
+ MPU60X0_I2C_CONF_I2C_MST_EN,
+ MPU60X0_I2C_CONF_DONE
+};
+
struct Mpu60x0_I2c {
struct i2c_periph *i2c_p;
struct i2c_transaction i2c_trans;
@@ -48,7 +63,9 @@ struct Mpu60x0_I2c {
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
int16_t value[3]; ///< rates data values accessible by channel index
} data_rates;
+ uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN];
struct Mpu60x0Config config;
+ enum Mpu60x0I2cSlaveInitStatus slave_init_status;
};
// Functions
diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h
index 211d1729f0..ac48968ab6 100644
--- a/sw/airborne/peripherals/mpu60x0_regs.h
+++ b/sw/airborne/peripherals/mpu60x0_regs.h
@@ -41,21 +41,21 @@
#define MPU60X0_REG_PWR_MGMT_2 0x6C
// FIFO
-#define MPU60X0_REG_FIFO_EN 0X23
+#define MPU60X0_REG_FIFO_EN 0x23
#define MPU60X0_REG_FIFO_COUNT_H 0x72
#define MPU60X0_REG_FIFO_COUNT_L 0x73
#define MPU60X0_REG_FIFO_R_W 0x74
// Measurement Settings
-#define MPU60X0_REG_SMPLRT_DIV 0X19
-#define MPU60X0_REG_CONFIG 0X1A
-#define MPU60X0_REG_GYRO_CONFIG 0X1B
-#define MPU60X0_REG_ACCEL_CONFIG 0X1C
+#define MPU60X0_REG_SMPLRT_DIV 0x19
+#define MPU60X0_REG_CONFIG 0x1A
+#define MPU60X0_REG_GYRO_CONFIG 0x1B
+#define MPU60X0_REG_ACCEL_CONFIG 0x1C
// I2C Slave settings
-#define MPU60X0_REG_I2C_MST_CTRL 0X24
-#define MPU60X0_REG_I2C_MST_STATUS 0X36
-#define MPU60X0_REG_I2C_MST_DELAY 0X67
+#define MPU60X0_REG_I2C_MST_CTRL 0x24
+#define MPU60X0_REG_I2C_MST_STATUS 0x36
+#define MPU60X0_REG_I2C_MST_DELAY 0x67
// Slave 0
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
@@ -84,44 +84,51 @@
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
// Interrupt
-#define MPU60X0_REG_INT_PIN_CFG 0X37
-#define MPU60X0_REG_INT_ENABLE 0X38
-#define MPU60X0_REG_INT_STATUS 0X3A
+#define MPU60X0_REG_INT_PIN_CFG 0x37
+#define MPU60X0_REG_INT_ENABLE 0x38
+#define MPU60X0_REG_INT_STATUS 0x3A
// Accelero
-#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
-#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
-#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
-#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
-#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
-#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
+#define MPU60X0_REG_ACCEL_XOUT_H 0x3B
+#define MPU60X0_REG_ACCEL_XOUT_L 0x3C
+#define MPU60X0_REG_ACCEL_YOUT_H 0x3D
+#define MPU60X0_REG_ACCEL_YOUT_L 0x3E
+#define MPU60X0_REG_ACCEL_ZOUT_H 0x3F
+#define MPU60X0_REG_ACCEL_ZOUT_L 0x40
// Temperature
-#define MPU60X0_REG_TEMP_OUT_H 0X41
-#define MPU60X0_REG_TEMP_OUT_L 0X42
+#define MPU60X0_REG_TEMP_OUT_H 0x41
+#define MPU60X0_REG_TEMP_OUT_L 0x42
// Gyro
-#define MPU60X0_REG_GYRO_XOUT_H 0X43
-#define MPU60X0_REG_GYRO_XOUT_L 0X44
-#define MPU60X0_REG_GYRO_YOUT_H 0X45
-#define MPU60X0_REG_GYRO_YOUT_L 0X46
-#define MPU60X0_REG_GYRO_ZOUT_H 0X47
-#define MPU60X0_REG_GYRO_ZOUT_L 0X48
+#define MPU60X0_REG_GYRO_XOUT_H 0x43
+#define MPU60X0_REG_GYRO_XOUT_L 0x44
+#define MPU60X0_REG_GYRO_YOUT_H 0x45
+#define MPU60X0_REG_GYRO_YOUT_L 0x46
+#define MPU60X0_REG_GYRO_ZOUT_H 0x47
+#define MPU60X0_REG_GYRO_ZOUT_L 0x48
// External Sensor Data
-#define MPU60X0_EXT_SENS_DATA 0X49
+#define MPU60X0_EXT_SENS_DATA 0x49
#define MPU60X0_EXT_SENS_DATA_SIZE 24
-/////////////////////////////////////////////////
-// MPU60X0 Definitions
-
-#define MPU60X0_REG_WHO_AM_I 0X75
+#define MPU60X0_REG_WHO_AM_I 0x75
#define MPU60X0_WHOAMI_REPLY 0x68
// Bit positions
#define MPU60X0_I2C_BYPASS_EN 1
+
+// in MPU60X0_REG_USER_CTRL
+#define MPU60X0_SIG_COND_RESET 0
+#define MPU60X0_I2C_MST_RESET 1
+#define MPU60X0_FIFO_RESET 2
+#define MPU60X0_I2C_IF_DIS 4
#define MPU60X0_I2C_MST_EN 5
+#define MPU60X0_FIFO_EN 6
+
+// in MPU60X0_REG_I2C_MST_STATUS
+#define MPU60X0_I2C_SLV4_DONE 6
/** Digital Low Pass Filter Options
* DLFP is affecting both gyro and accels,
@@ -157,4 +164,26 @@ enum Mpu60x0AccelRanges {
MPU60X0_ACCEL_RANGE_16G = 0x03
};
+/**
+ * I2C Master clock
+ */
+enum Mpu60x0MstClk {
+ MPU60X0_MST_CLK_500KHZ = 0x9,
+ MPU60X0_MST_CLK_471KHZ = 0xA,
+ MPU60X0_MST_CLK_444KHZ = 0xB,
+ MPU60X0_MST_CLK_421KHZ = 0xC,
+ MPU60X0_MST_CLK_400KHZ = 0xD,
+ MPU60X0_MST_CLK_381KHZ = 0xE,
+ MPU60X0_MST_CLK_364KHZ = 0xF,
+ MPU60X0_MST_CLK_348KHZ = 0x0,
+ MPU60X0_MST_CLK_333KHZ = 0x1,
+ MPU60X0_MST_CLK_320KHZ = 0x2,
+ MPU60X0_MST_CLK_308KHZ = 0x3,
+ MPU60X0_MST_CLK_296KHZ = 0x4,
+ MPU60X0_MST_CLK_286KHZ = 0x5,
+ MPU60X0_MST_CLK_276KHZ = 0x6,
+ MPU60X0_MST_CLK_267KHZ = 0x7,
+ MPU60X0_MST_CLK_254KHZ = 0x8
+};
+
#endif /* MPU60X0_REGS_H */
diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
index 97993f322e..0689e972ad 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.c
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -42,7 +42,7 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t
mpu->spi_trans.select = SPISelectUnselect;
mpu->spi_trans.slave_idx = slave_idx;
- mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME
+ mpu->spi_trans.output_length = 2;
mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN;
mpu->spi_trans.before_cb = NULL;
mpu->spi_trans.after_cb = NULL;
@@ -58,6 +58,8 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t
mpu->data_available = FALSE;
mpu->config.initialized = FALSE;
mpu->config.init_status = MPU60X0_CONF_UNINIT;
+
+ mpu->slave_init_status = MPU60X0_SPI_CONF_UNINIT;
}
@@ -85,9 +87,9 @@ void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
{
if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
mpu->spi_trans.output_length = 1;
- mpu->spi_trans.input_length = 16; // FIXME external data
+ mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes;
/* set read bit and multiple byte bit, then address */
- mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
+ mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS | MPU60X0_SPI_READ;
spi_submit(mpu->spi_p, &(mpu->spi_trans));
}
}
@@ -102,14 +104,19 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
}
else if (mpu->spi_trans.status == SPITransSuccess) {
// Successfull reading
- if (bit_is_set(mpu->rx_buf[1],0)) {
+ if (bit_is_set(mpu->rx_buf[1], 0)) {
// new data
- mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,2);
- mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,4);
- mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,6);
- mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,10);
- mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,12);
- mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,14);
+ mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2);
+ mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4);
+ mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6);
+ mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10);
+ mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12);
+ mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
+
+ // if we are reading slaves, copy the ext_sens_data
+ if (mpu->config.nb_slaves > 0)
+ memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15);
+
mpu->data_available = TRUE;
}
mpu->spi_trans.status = SPITransDone;
@@ -121,12 +128,50 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
mpu->config.init_status--; // Retry config (TODO max retry)
case SPITransSuccess:
case SPITransDone:
- mpu->spi_trans.status = SPITransDone;
mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config));
- if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone;
+ if (mpu->config.initialized)
+ mpu->spi_trans.status = SPITransDone;
break;
default:
break;
}
}
}
+
+/** @todo: only one slave so far. */
+bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu)
+{
+ struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu);
+
+ if (mpu_spi->slave_init_status == MPU60X0_SPI_CONF_UNINIT)
+ mpu_spi->slave_init_status++;
+
+ switch (mpu_spi->slave_init_status) {
+ case MPU60X0_SPI_CONF_I2C_MST_CLK:
+ /* configure MPU I2C master clock and stop/start between slave reads */
+ mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk));
+ mpu_spi->slave_init_status++;
+ break;
+ case MPU60X0_SPI_CONF_I2C_MST_DELAY:
+ /* Set I2C slaves delayed sample rate */
+ mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_spi->config.i2c_mst_delay);
+ mpu_spi->slave_init_status++;
+ break;
+ case MPU60X0_SPI_CONF_I2C_MST_EN:
+ /* enable internal I2C master and disable primary I2C interface */
+ mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_I2C_IF_DIS) |
+ (1 << MPU60X0_I2C_MST_EN)));
+ mpu_spi->slave_init_status++;
+ break;
+ case MPU60X0_SPI_CONF_SLAVES_CONFIGURE:
+ /* configure first slave, only one slave supported so far */
+ if (mpu_spi->config.slaves[0].configure(mpu_set, mpu))
+ mpu_spi->slave_init_status++;
+ break;
+ case MPU60X0_SPI_CONF_DONE:
+ return TRUE;
+ default:
+ break;
+ }
+ return FALSE;
+}
diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h
index 6eac22267c..75975f851b 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.h
+++ b/sw/airborne/peripherals/mpu60x0_spi.h
@@ -37,12 +37,22 @@
#define MPU60X0_BUFFER_LEN 32
+#define MPU60X0_BUFFER_EXT_LEN 16
+
+enum Mpu60x0SpiSlaveInitStatus {
+ MPU60X0_SPI_CONF_UNINIT,
+ MPU60X0_SPI_CONF_I2C_MST_CLK,
+ MPU60X0_SPI_CONF_I2C_MST_DELAY,
+ MPU60X0_SPI_CONF_I2C_MST_EN,
+ MPU60X0_SPI_CONF_SLAVES_CONFIGURE,
+ MPU60X0_SPI_CONF_DONE
+};
struct Mpu60x0_Spi {
struct spi_periph *spi_p;
struct spi_transaction spi_trans;
- volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length
- volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem
+ volatile uint8_t tx_buf[2];
+ volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN];
volatile bool_t data_available; ///< data ready flag
union {
struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
@@ -52,7 +62,9 @@ struct Mpu60x0_Spi {
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
int16_t value[3]; ///< rates data values accessible by channel index
} data_rates;
+ uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN];
struct Mpu60x0Config config;
+ enum Mpu60x0SpiSlaveInitStatus slave_init_status;
};
// Functions
diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
index 491d53b18e..0f04e5b27a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
@@ -33,9 +33,13 @@
#include "boards/ardrone/at_com.h"
#include "subsystems/electrical.h"
+#ifdef USE_GPS_ARDRONE2
+#include "subsystems/gps/gps_ardrone2.h"
+#endif
+
struct AhrsARDrone ahrs_impl;
struct AhrsAligner ahrs_aligner;
-unsigned char buffer[2048]; //Packet buffer
+unsigned char buffer[4096]; //Packet buffer
#if DOWNLINK
#include "subsystems/datalink/telemetry.h"
@@ -80,6 +84,12 @@ void ahrs_propagate(void) {
//Recieve the main packet
at_com_recieve_navdata(buffer);
navdata_t* main_packet = (navdata_t*) &buffer;
+
+ //When this isn't a valid packet return
+ if(main_packet->header != NAVDATA_HEADER)
+ return;
+
+ //Set the state
ahrs_impl.state = main_packet->ardrone_state;
//Init the option
@@ -88,10 +98,11 @@ void ahrs_propagate(void) {
//The possible packets
navdata_demo_t* navdata_demo;
+ navdata_gps_t* navdata_gps;
navdata_phys_measures_t* navdata_phys_measures;
//Read the navdata until packet is fully readed
- while(!full_read) {
+ while(!full_read && navdata_option->size > 0) {
//Check the tag for the right option
switch(navdata_option->tag) {
case 0: //NAVDATA_DEMO
@@ -124,6 +135,14 @@ void ahrs_propagate(void) {
//Set the AHRS accel state
INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
break;
+#ifdef USE_GPS_ARDRONE2
+ case 27: //NAVDATA_GPS
+ navdata_gps = (navdata_gps_t*) navdata_option;
+
+ // Send the data to the gps parser
+ gps_ardrone2_parse(navdata_gps);
+ break;
+#endif
case 0xFFFF: //CHECKSUM
//TODO: Check the checksum
full_read = TRUE;
@@ -132,7 +151,7 @@ void ahrs_propagate(void) {
//printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
break;
}
- navdata_option = (navdata_option_t*) ((int)navdata_option + navdata_option->size);
+ navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
}
}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
index ad506d136c..2ad5bc5d43 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
@@ -23,7 +23,7 @@
/**
* @file subsystems/ahrs/ahrs_float_mlkf.c
*
- * Mulitplicative linearized Kalman Filter in quaternion formulation.
+ * Multiplicative linearized Kalman Filter in quaternion formulation.
*
* Estimate the attitude, heading and gyro bias.
*/
@@ -46,6 +46,10 @@
//#include
+#ifndef AHRS_PROPAGATE_FREQUENCY
+#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
+#endif
+
static inline void propagate_ref(void);
static inline void propagate_state(void);
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]);
@@ -144,10 +148,12 @@ static inline void propagate_ref(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
- // printf("propagate_ref %f\n", gyro_float.p);
+
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
+
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
+ /* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
index 5fa1b729f8..3b5552fcad 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
@@ -21,9 +21,9 @@
*/
/**
- * @file subsystems/ahrs/ahrs_float_mlkf.h
+ * @file subsystems/ahrs/ahrs_float_mlkf_opt.h
*
- * Mulitplicative linearized Kalman Filter in quaternion formulation.
+ * Multiplicative linearized Kalman Filter in quaternion formulation.
*
* Estimate the attitude, heading and gyro bias.
*/
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
index 7ed3b06cb5..d0075cdd0c 100644
--- a/sw/airborne/subsystems/electrical.c
+++ b/sw/airborne/subsystems/electrical.c
@@ -55,7 +55,7 @@ void electrical_init(void) {
#endif
/* measure current if available, otherwise estimate it */
-#if defined ADC_CHANNEL_CURRENT
+#if defined ADC_CHANNEL_CURRENT && !defined SITL
adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE);
#elif defined MILLIAMP_AT_FULL_THROTTLE
PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c
new file mode 100644
index 0000000000..60a1e5db78
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_ardrone2.c
@@ -0,0 +1,63 @@
+/*
+ *
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file subsystems/gps/gps_ardrone2.c
+ * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
+ */
+
+#include "subsystems/gps.h"
+#include "math/pprz_geodetic_double.h"
+
+bool_t gps_ardrone2_available;
+
+void gps_impl_init( void ) {
+ gps_ardrone2_available = FALSE;
+}
+
+void gps_ardrone2_parse(navdata_gps_t *navdata_gps) {
+ // Set the lla double struct from the navdata
+ struct LlaCoor_d gps_lla_d;
+ gps_lla_d.lat = RadOfDeg(navdata_gps->lat);
+ gps_lla_d.lon = RadOfDeg(navdata_gps->lon);
+ gps_lla_d.alt = navdata_gps->elevation;
+
+ // Convert it to ecef
+ struct EcefCoor_d gps_ecef_d;
+ ecef_of_lla_d(&gps_ecef_d, &gps_lla_d);
+
+ // Convert the lla and ecef to int and set them in gps
+ ECEF_BFP_OF_REAL(gps.ecef_pos, gps_ecef_d);
+ LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d);
+
+ // TODO: parse other stuff
+
+ // Check if we have a fix TODO: check if 2D or 3D fix?
+ if (navdata_gps->gps_state == 1)
+ gps.fix = GPS_FIX_3D;
+ else
+ gps.fix = GPS_FIX_NONE;
+
+ // Set that there is a packet
+ gps_ardrone2_available = TRUE;
+}
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h
new file mode 100644
index 0000000000..42e926ff34
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_ardrone2.h
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file subsystems/gps/gps_ardrone2.h
+ * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
+ *
+*/
+
+#ifndef GPS_ARDRONE_H
+#define GPS_ARDRONE_H
+
+#include "boards/ardrone/at_com.h"
+
+//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet
+extern bool_t gps_ardrone2_available;
+
+/*
+ * The GPS event
+ */
+#define GpsEvent(_sol_available_callback) { \
+ if (gps_ardrone2_available) { \
+ if (gps.fix == GPS_FIX_3D) { \
+ gps.last_fix_ticks = sys_time.nb_sec_rem; \
+ gps.last_fix_time = sys_time.nb_sec; \
+ } \
+ _sol_available_callback(); \
+ gps_ardrone2_available = FALSE; \
+ } \
+ }
+
+void gps_ardrone2_parse(navdata_gps_t *navdata_gps);
+
+/* Maybe needed?
+#define gps_nmea_Reset(_val) { }
+*/
+
+#endif /* GPS_ARDRONE_H */
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.c b/sw/airborne/subsystems/imu/imu_aspirin_2.c
deleted file mode 100644
index 33a444456e..0000000000
--- a/sw/airborne/subsystems/imu/imu_aspirin_2.c
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * Copyright (C) 2013 Felix Ruess
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file subsystems/imu/imu_aspirin_2.c
- * Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
- */
-
-#include "subsystems/imu.h"
-
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/spi.h"
-
-
-/* defaults suitable for Lisa */
-#ifndef ASPIRIN_2_SPI_SLAVE_IDX
-#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
-#endif
-PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
-
-#ifndef ASPIRIN_2_SPI_DEV
-#define ASPIRIN_2_SPI_DEV spi2
-#endif
-PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
-
-#ifndef ASPIRIN_2_I2C_DEV
-#define ASPIRIN_2_I2C_DEV i2c2
-#endif
-PRINT_CONFIG_VAR(ASPIRIN_2_I2C_DEV)
-
-
-/* gyro internal lowpass frequency */
-#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
-#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
-#define ASPIRIN_2_SMPLRT_DIV 1
-PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
-#endif
-PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
-PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
-
-#ifndef ASPIRIN_2_GYRO_RANGE
-#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
-#endif
-PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
-
-#ifndef ASPIRIN_2_ACCEL_RANGE
-#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
-#endif
-PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
-
-
-struct ImuAspirin2 imu_aspirin2;
-
-void imu_impl_init(void)
-{
- imu_aspirin2.accel_valid = FALSE;
- imu_aspirin2.gyro_valid = FALSE;
- imu_aspirin2.mag_valid = FALSE;
-
- mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
- // change the default configuration
- imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
- imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
- imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
- imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
- //imu_aspirin2.mpu.config.i2c_bypass = FALSE;
- //imu_aspirin2.mpu.config.drdy_int_enable = TRUE;
-
- //hmc58xx_init(&imu_aspirin2.mag_hmc, &(ASPIRIN_2_I2C_DEV), HMC58XX_ADDR);
-}
-
-
-void imu_periodic(void)
-{
- mpu60x0_spi_periodic(&imu_aspirin2.mpu);
-
- // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
- //RunOnceEvery(10, hmc58xx_periodic(&imu_aspirin2.mag_hmc));
-}
-
-void imu_aspirin2_event(void)
-{
- mpu60x0_spi_event(&imu_aspirin2.mpu);
- if (imu_aspirin2.mpu.data_available) {
- RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
- VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
- imu_aspirin2.mpu.data_available = FALSE;
- imu_aspirin2.gyro_valid = TRUE;
- imu_aspirin2.accel_valid = TRUE;
- }
-
-#if 0
- /* HMC58XX event task */
- hmc58xx_event(&imu_aspirin2.mag_hmc);
- if (imu_aspirin2.mag_hmc.data_available) {
- imu.mag_unscaled.x = imu_aspirin2.mag_hmc.data.vect.y;
- imu.mag_unscaled.y = -imu_aspirin2.mag_hmc.data.vect.x;
- imu.mag_unscaled.z = imu_aspirin2.mag_hmc.data.vect.z;
- imu_aspirin2.mag_hmc.data_available = FALSE;
- imu_aspirin2.mag_valid = TRUE;
- }
-#endif
-}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
new file mode 100644
index 0000000000..84918649e4
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
@@ -0,0 +1,232 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_aspirin_2_spi.c
+ * Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
+ */
+
+#include "subsystems/imu.h"
+
+#include "mcu_periph/spi.h"
+#include "peripherals/hmc58xx_regs.h"
+
+/* defaults suitable for Lisa */
+#ifndef ASPIRIN_2_SPI_SLAVE_IDX
+#define ASPIRIN_2_SPI_SLAVE_IDX SPI_SLAVE2
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
+
+#ifndef ASPIRIN_2_SPI_DEV
+#define ASPIRIN_2_SPI_DEV spi2
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
+
+/* gyro internal lowpass frequency */
+#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
+#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
+#define ASPIRIN_2_SMPLRT_DIV 1
+//PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
+PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
+
+#ifndef ASPIRIN_2_GYRO_RANGE
+#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
+
+#ifndef ASPIRIN_2_ACCEL_RANGE
+#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
+#endif
+PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
+
+
+/* HMC58XX default conf */
+#ifndef HMC58XX_DO
+#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 75Hz with HMC5883)
+#endif
+#ifndef HMC58XX_MS
+#define HMC58XX_MS 0x0 // Measurement configuration
+#endif
+#ifndef HMC58XX_GN
+#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss)
+#endif
+#ifndef HMC58XX_MD
+#define HMC58XX_MD 0x0 // Continious measurement mode
+#endif
+
+#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS))
+#define HMC58XX_CRB (HMC58XX_GN<<5)
+
+
+struct ImuAspirin2Spi imu_aspirin2;
+
+void mpu_wait_slave4_ready(void);
+void mpu_wait_slave4_ready_cb(struct spi_transaction * t);
+bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu);
+
+void imu_impl_init(void)
+{
+ imu_aspirin2.accel_valid = FALSE;
+ imu_aspirin2.gyro_valid = FALSE;
+ imu_aspirin2.mag_valid = FALSE;
+
+ mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
+ // change the default configuration
+ imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
+ imu_aspirin2.mpu.config.dlpf_cfg = ASPIRIN_2_LOWPASS_FILTER;
+ imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE;
+ imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE;
+
+ /* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */
+ imu_aspirin2.mpu.config.nb_bytes = 21;
+
+ /* HMC5883 magnetometer as I2C slave */
+ imu_aspirin2.mpu.config.nb_slaves = 1;
+
+ /* set function to configure mag */
+ imu_aspirin2.mpu.config.slaves[0].configure = &imu_aspirin2_configure_mag_slave;
+
+
+ /* Set MPU I2C master clock */
+ imu_aspirin2.mpu.config.i2c_mst_clk = MPU60X0_MST_CLK_400KHZ;
+ /* Enable I2C slave0 delayed sample rate */
+ imu_aspirin2.mpu.config.i2c_mst_delay = 1;
+
+
+ /* configure spi transaction for wait_slave4 */
+ imu_aspirin2.wait_slave4_trans.cpol = SPICpolIdleHigh;
+ imu_aspirin2.wait_slave4_trans.cpha = SPICphaEdge2;
+ imu_aspirin2.wait_slave4_trans.dss = SPIDss8bit;
+ imu_aspirin2.wait_slave4_trans.bitorder = SPIMSBFirst;
+ imu_aspirin2.wait_slave4_trans.cdiv = SPIDiv64;
+
+ imu_aspirin2.wait_slave4_trans.select = SPISelectUnselect;
+ imu_aspirin2.wait_slave4_trans.slave_idx = ASPIRIN_2_SPI_SLAVE_IDX;
+ imu_aspirin2.wait_slave4_trans.output_length = 1;
+ imu_aspirin2.wait_slave4_trans.input_length = 2;
+ imu_aspirin2.wait_slave4_trans.before_cb = NULL;
+ imu_aspirin2.wait_slave4_trans.after_cb = mpu_wait_slave4_ready_cb;
+ imu_aspirin2.wait_slave4_trans.input_buf = &(imu_aspirin2.wait_slave4_rx_buf[0]);
+ imu_aspirin2.wait_slave4_trans.output_buf = &(imu_aspirin2.wait_slave4_tx_buf[0]);
+
+ imu_aspirin2.wait_slave4_trans.status = SPITransDone;
+ imu_aspirin2.slave4_ready = FALSE;
+}
+
+
+void imu_periodic(void)
+{
+ mpu60x0_spi_periodic(&imu_aspirin2.mpu);
+}
+
+#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
+
+void imu_aspirin2_event(void)
+{
+ mpu60x0_spi_event(&imu_aspirin2.mpu);
+ if (imu_aspirin2.mpu.data_available) {
+ struct Int32Vect3 mag;
+ mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
+ mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
+ mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
+#ifdef LISA_M_LONGITUDINAL_X
+ RATES_ASSIGN(imu.gyro_unscaled,
+ imu_aspirin2.mpu.data_rates.rates.q,
+ -imu_aspirin2.mpu.data_rates.rates.p,
+ imu_aspirin2.mpu.data_rates.rates.r);
+ VECT3_ASSIGN(imu.accel_unscaled,
+ imu_aspirin2.mpu.data_accel.vect.y,
+ -imu_aspirin2.mpu.data_accel.vect.x,
+ imu_aspirin2.mpu.data_accel.vect.z);
+ VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.z, mag.y);
+#else
+ RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
+ VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
+ VECT3_ASSIGN(imu.mag_unscaled, mag.z, -mag.x, mag.y)
+#endif
+ imu_aspirin2.mpu.data_available = FALSE;
+ imu_aspirin2.gyro_valid = TRUE;
+ imu_aspirin2.accel_valid = TRUE;
+ imu_aspirin2.mag_valid = TRUE;
+ }
+}
+
+// hack with waiting to avoid creating another event loop to check the mag config status
+static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void* mpu, uint8_t _reg, uint8_t _val)
+{
+ mpu_set(mpu, _reg, _val);
+ while(imu_aspirin2.mpu.spi_trans.status != SPITransSuccess);
+}
+
+/** function to configure hmc5883 mag
+ * @return TRUE if mag configuration finished
+ */
+bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu)
+{
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
+
+ mpu_wait_slave4_ready();
+
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
+
+ mpu_wait_slave4_ready();
+
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1));
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_CTRL, (1 << 7)); // Slave 4 enable
+
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ);
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM);
+ // Put the enable command as last.
+ mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_CTRL,
+ (1 << 7) | // Slave 0 enable
+ (6 << 0) ); // Read 6 bytes
+
+ return TRUE;
+}
+
+void mpu_wait_slave4_ready(void)
+{
+ while (!imu_aspirin2.slave4_ready) {
+ if (imu_aspirin2.wait_slave4_trans.status == SPITransDone) {
+ imu_aspirin2.wait_slave4_tx_buf[0] = MPU60X0_REG_I2C_MST_STATUS | MPU60X0_SPI_READ;
+ spi_submit(imu_aspirin2.mpu.spi_p, &(imu_aspirin2.wait_slave4_trans));
+ }
+ }
+}
+
+void mpu_wait_slave4_ready_cb(struct spi_transaction * t)
+{
+ if (bit_is_set(t->input_buf[1], MPU60X0_I2C_SLV4_DONE))
+ imu_aspirin2.slave4_ready = TRUE;
+ else
+ imu_aspirin2.slave4_ready = FALSE;
+ t->status = SPITransDone;
+}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
new file mode 100644
index 0000000000..9ab98631ca
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_aspirin_2_spi.h
+ * Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
+ */
+
+#ifndef IMU_ASPIRIN_2_SPI_H
+#define IMU_ASPIRIN_2_SPI_H
+
+#include "std.h"
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "subsystems/imu/imu_mpu60x0_defaults.h"
+#include "peripherals/mpu60x0_spi.h"
+
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+
+struct ImuAspirin2Spi {
+ volatile bool_t gyro_valid;
+ volatile bool_t accel_valid;
+ volatile bool_t mag_valid;
+ struct Mpu60x0_Spi mpu;
+
+ struct spi_transaction wait_slave4_trans;
+ volatile uint8_t wait_slave4_tx_buf[1];
+ volatile uint8_t wait_slave4_rx_buf[2];
+ volatile bool_t slave4_ready;
+};
+
+extern struct ImuAspirin2Spi imu_aspirin2;
+
+extern void imu_aspirin2_event(void);
+
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ imu_aspirin2_event();
+ if (imu_aspirin2.gyro_valid) {
+ imu_aspirin2.gyro_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_aspirin2.accel_valid) {
+ imu_aspirin2.accel_valid = FALSE;
+ _accel_handler();
+ }
+ if (imu_aspirin2.mag_valid) {
+ imu_aspirin2.mag_valid = FALSE;
+ _mag_handler();
+ }
+}
+
+#endif /* IMU_ASPIRIN_2_H */
diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
new file mode 100644
index 0000000000..39711a6ce9
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_drotek_10dof_v2.c
+ *
+ * Driver for the Drotek 10DOF V2 IMU.
+ * MPU6050 + HMC5883 + MS5611
+ * Reading the baro is not part of the IMU driver.
+ *
+ * By default the axes orientation should be as printed on the pcb,
+ * meaning z-axis pointing down if ICs are facing down.
+ * The orientation can be switched so that the IMU can be mounted ICs facing up
+ * by defining IMU_DROTEK_2_ORIENTATION_IC_UP.
+ */
+
+#include "subsystems/imu.h"
+
+#include "mcu_periph/i2c.h"
+
+#if !defined DROTEK_2_LOWPASS_FILTER && !defined DROTEK_2_SMPLRT_DIV
+#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ
+#define DROTEK_2_SMPLRT_DIV 9
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz")
+#endif
+PRINT_CONFIG_VAR(DROTEK_2_SMPLRT_DIV)
+PRINT_CONFIG_VAR(DROTEK_2_LOWPASS_FILTER)
+
+#ifndef DROTEK_2_GYRO_RANGE
+#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
+#endif
+PRINT_CONFIG_VAR(DROTEK_2_GYRO_RANGE)
+
+#ifndef DROTEK_2_ACCEL_RANGE
+#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
+#endif
+PRINT_CONFIG_VAR(DROTEK_2_ACCEL_RANGE)
+
+#ifndef DROTEK_2_MPU_I2C_ADDR
+#define DROTEK_2_MPU_I2C_ADDR MPU60X0_ADDR_ALT
+#endif
+PRINT_CONFIG_VAR(DROTEK_2_MPU_I2C_ADDR)
+
+#ifndef DROTEK_2_HMC_I2C_ADDR
+#define DROTEK_2_HMC_I2C_ADDR HMC58XX_ADDR
+#endif
+PRINT_CONFIG_VAR(DROTEK_2_HMC_I2C_ADDR)
+
+
+struct ImuDrotek2 imu_drotek2;
+
+void imu_impl_init(void)
+{
+ /* MPU-60X0 */
+ mpu60x0_i2c_init(&imu_drotek2.mpu, &(DROTEK_2_I2C_DEV), DROTEK_2_MPU_I2C_ADDR);
+ // change the default configuration
+ imu_drotek2.mpu.config.smplrt_div = DROTEK_2_SMPLRT_DIV;
+ imu_drotek2.mpu.config.dlpf_cfg = DROTEK_2_LOWPASS_FILTER;
+ imu_drotek2.mpu.config.gyro_range = DROTEK_2_GYRO_RANGE;
+ imu_drotek2.mpu.config.accel_range = DROTEK_2_ACCEL_RANGE;
+
+ /* HMC5883 magnetometer */
+ hmc58xx_init(&imu_drotek2.hmc, &(DROTEK_2_I2C_DEV), DROTEK_2_HMC_I2C_ADDR);
+
+ /* mag is declared as slave to call the configure function,
+ * regardless if it is an actual MPU slave or passthrough
+ */
+ imu_drotek2.mpu.config.nb_slaves = 1;
+ /* set callback function to configure mag */
+ imu_drotek2.mpu.config.slaves[0].configure = &imu_drotek2_configure_mag_slave;
+
+ // use hmc mag via passthrough
+ imu_drotek2.mpu.config.i2c_bypass = TRUE;
+
+ imu_drotek2.gyro_valid = FALSE;
+ imu_drotek2.accel_valid = FALSE;
+ imu_drotek2.mag_valid = FALSE;
+}
+
+void imu_periodic(void)
+{
+ // Start reading the latest gyroscope data
+ mpu60x0_i2c_periodic(&imu_drotek2.mpu);
+
+ // Read HMC58XX at ~50Hz (main loop for rotorcraft: 512Hz)
+ if (imu_drotek2.mpu.config.initialized) {
+ RunOnceEvery(10, hmc58xx_read(&imu_drotek2.hmc));
+ }
+}
+
+void imu_drotek2_event(void)
+{
+ // If the MPU6050 I2C transaction has succeeded: convert the data
+ mpu60x0_i2c_event(&imu_drotek2.mpu);
+
+ if (imu_drotek2.mpu.data_available) {
+#if IMU_DROTEK_2_ORIENTATION_IC_UP
+ /* change orientation, so if ICs face up, z-axis is down */
+ imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
+ imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
+ imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
+ imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
+ imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
+ imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
+#else
+ /* default orientation as should be printed on the pcb, z-down, ICs down */
+ RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
+ VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
+#endif
+
+ imu_drotek2.mpu.data_available = FALSE;
+ imu_drotek2.gyro_valid = TRUE;
+ imu_drotek2.accel_valid = TRUE;
+ }
+
+ /* HMC58XX event task */
+ hmc58xx_event(&imu_drotek2.hmc);
+ if (imu_drotek2.hmc.data_available) {
+#if IMU_DROTEK_2_ORIENTATION_IC_UP
+ imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
+ imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
+ imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
+#else
+ VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
+#endif
+ imu_drotek2.hmc.data_available = FALSE;
+ imu_drotek2.mag_valid = TRUE;
+ }
+}
+
+/** callback function to configure hmc5883 mag
+ * @return TRUE if mag configuration finished
+ */
+bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused)))
+{
+ hmc58xx_start_configure(&imu_drotek2.hmc);
+ if (imu_drotek2.hmc.initialized)
+ return TRUE;
+ else
+ return FALSE;
+}
diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
new file mode 100644
index 0000000000..6dd97abce9
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_drotek_10dof_v2.h
+ *
+ * Driver for the Drotek 10DOF V2 IMU.
+ * MPU6050 + HMC5883 + MS5611
+ */
+
+#ifndef IMU_DROTEK_10DOF_V2_H
+#define IMU_DROTEK_10DOF_V2_H
+
+#include "std.h"
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "peripherals/mpu60x0_i2c.h"
+#include "peripherals/hmc58xx.h"
+
+
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+
+
+/** default gyro sensitivy and neutral from the datasheet
+ * MPU with 1000 deg/s has 32.8 LSB/(deg/s)
+ * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/32.8 * pi/180 * 4096 = 2.17953
+ I*/
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+// FIXME
+#define IMU_GYRO_P_SENS 2.17953
+#define IMU_GYRO_P_SENS_NUM 18271
+#define IMU_GYRO_P_SENS_DEN 8383
+#define IMU_GYRO_Q_SENS 2.17953
+#define IMU_GYRO_Q_SENS_NUM 18271
+#define IMU_GYRO_Q_SENS_DEN 8383
+#define IMU_GYRO_R_SENS 2.17953
+#define IMU_GYRO_R_SENS_NUM 18271
+#define IMU_GYRO_R_SENS_DEN 8383
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+/** default accel sensitivy from the datasheet
+ * MPU with 8g has 4096 LSB/g
+ * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+// FIXME
+#define IMU_ACCEL_X_SENS 2.4525
+#define IMU_ACCEL_X_SENS_NUM 981
+#define IMU_ACCEL_X_SENS_DEN 400
+#define IMU_ACCEL_Y_SENS 2.4525
+#define IMU_ACCEL_Y_SENS_NUM 981
+#define IMU_ACCEL_Y_SENS_DEN 400
+#define IMU_ACCEL_Z_SENS 2.4525
+#define IMU_ACCEL_Z_SENS_NUM 981
+#define IMU_ACCEL_Z_SENS_DEN 400
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
+
+struct ImuDrotek2 {
+ volatile bool_t gyro_valid;
+ volatile bool_t accel_valid;
+ volatile bool_t mag_valid;
+ struct Mpu60x0_I2c mpu;
+ struct Hmc58xx hmc;
+};
+
+extern struct ImuDrotek2 imu_drotek2;
+
+extern void imu_drotek2_event(void);
+extern bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu);
+
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ imu_drotek2_event();
+ if (imu_drotek2.gyro_valid) {
+ imu_drotek2.gyro_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_drotek2.accel_valid) {
+ imu_drotek2.accel_valid = FALSE;
+ _accel_handler();
+ }
+ if (imu_drotek2.mag_valid) {
+ imu_drotek2.mag_valid = FALSE;
+ _mag_handler();
+ }
+}
+
+#endif /* IMU_DROTEK_10DOF_V2_H */
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2.h b/sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
similarity index 62%
rename from sw/airborne/subsystems/imu/imu_aspirin_2.h
rename to sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
index 33b91686e4..b53faef232 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin_2.h
+++ b/sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
@@ -20,35 +20,14 @@
*/
/**
- * @file subsystems/imu/imu_aspirin_2.h
- * Driver for the Aspirin v2.x IMU using SPI for the MPU6000.
+ * @file subsystems/imu/imu_mpu60x0_defaults.h
+ * Default sensitivity definitions for an IMU using the MPU60x0.
*/
-#ifndef IMU_ASPIRIN_2_H
-#define IMU_ASPIRIN_2_H
+#ifndef IMU_MPU60X0_DEFAULTS_H
+#define IMU_MPU60X0_DEFAULTS_H
-#include "std.h"
#include "generated/airframe.h"
-#include "subsystems/imu.h"
-
-#include "peripherals/mpu60x0_spi.h"
-
-#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
-#define IMU_MAG_X_SIGN 1
-#define IMU_MAG_Y_SIGN 1
-#define IMU_MAG_Z_SIGN 1
-#endif
-
-#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
-#define IMU_GYRO_P_SIGN 1
-#define IMU_GYRO_Q_SIGN 1
-#define IMU_GYRO_R_SIGN 1
-#endif
-#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
-#define IMU_ACCEL_X_SIGN 1
-#define IMU_ACCEL_Y_SIGN 1
-#define IMU_ACCEL_Z_SIGN 1
-#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
@@ -94,33 +73,4 @@
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
-
-struct ImuAspirin2 {
- volatile bool_t gyro_valid;
- volatile bool_t accel_valid;
- volatile bool_t mag_valid;
- struct Mpu60x0_Spi mpu;
-};
-
-extern struct ImuAspirin2 imu_aspirin2;
-
-extern void imu_aspirin2_event(void);
-
-
-static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
- imu_aspirin2_event();
- if (imu_aspirin2.gyro_valid) {
- imu_aspirin2.gyro_valid = FALSE;
- _gyro_handler();
- }
- if (imu_aspirin2.accel_valid) {
- imu_aspirin2.accel_valid = FALSE;
- _accel_handler();
- }
- if (imu_aspirin2.mag_valid) {
- imu_aspirin2.mag_valid = FALSE;
- _mag_handler();
- }
-}
-
-#endif /* IMU_ASPIRIN_2_H */
+#endif /* IMU_MPU60X0_DEFAULTS_H */
diff --git a/sw/airborne/subsystems/imu/imu_um6.c b/sw/airborne/subsystems/imu/imu_um6.c
index 00b3ba017a..079c3f34dc 100644
--- a/sw/airborne/subsystems/imu/imu_um6.c
+++ b/sw/airborne/subsystems/imu/imu_um6.c
@@ -26,7 +26,7 @@
*
* Takes care of configuration of the IMU, communication and parsing
* the received packets. See UM6 datasheet for configuration options.
- * Should be used with @ahrs_extern_euler AHRS subsystem.
+ * Should be used with ahrs_extern_euler AHRS subsystem.
*
* @author Michal Podhradsky
*/
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c
index 3f42c8085d..a50f0ed908 100644
--- a/sw/airborne/subsystems/ins/ins_ardrone2.c
+++ b/sw/airborne/subsystems/ins/ins_ardrone2.c
@@ -110,6 +110,7 @@ void ins_propagate() {
stateSetAccelNed_f(&ins_ltp_accel);
stateSetSpeedNed_f(&ins_ltp_speed);
+ //Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT
//Set the height and save the position
ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
@@ -138,11 +139,15 @@ void ins_update_gps(void) {
//Set the x and y and maybe z position in ltp and save
struct NedCoor_i ins_gps_pos_cm_ned;
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
+
+ //When we don't want to use the height of the navdata we can use the gps height
#if USE_GPS_HEIGHT
INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#else
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#endif
+
+ //Set the local origin
stateSetPositionNed_i(&ins_ltp_pos);
}
#endif /* USE_GPS */
diff --git a/sw/airborne/subsystems/navigation/traffic_info.c b/sw/airborne/subsystems/navigation/traffic_info.c
index 2d8fe65b10..faf3811eca 100644
--- a/sw/airborne/subsystems/navigation/traffic_info.c
+++ b/sw/airborne/subsystems/navigation/traffic_info.c
@@ -20,7 +20,7 @@
*/
/**
- * @file subsystems/naviation/traffic_info.c
+ * @file subsystems/navigation/traffic_info.c
*
* Information relative to the other aircrafts.
*
diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3
index 0bf8324c91..fb5c86db07 160000
--- a/sw/ext/libopencm3
+++ b/sw/ext/libopencm3
@@ -1 +1 @@
-Subproject commit 0bf8324c9128b146ec6a18d62d8f05a5b61a2530
+Subproject commit fb5c86db0723aa64740df7aebb3a0fd69acf52fe
diff --git a/sw/ext/luftboot b/sw/ext/luftboot
index 850436456f..3b6a352ba5 160000
--- a/sw/ext/luftboot
+++ b/sw/ext/luftboot
@@ -1 +1 @@
-Subproject commit 850436456f10e9c025a101b481077f547942d322
+Subproject commit 3b6a352ba55b576f576f0eb6b116d7eab0c99fcd
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index b88c683bc5..ac2da4c10b 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -66,9 +66,9 @@ external stick_init : int -> int = "ml_stick_init"
(** [stick_init device] Return 0 on success. Search for a device if [device]
is the empty string *)
-external stick_read : unit -> int * int * int array = "ml_stick_read"
-(** Return the number of buttons, an integer of bits for the buttons values
- and an array of signed integers for the axis *)
+external stick_read : unit -> int * int * int * int array = "ml_stick_read"
+(** Return the number of buttons, an integer of bits for the buttons values,
+ an integer for the hat and an array of signed integers for the axis *)
(** Range for the input axis *)
let max_input = 127
@@ -79,6 +79,7 @@ let trim_step = 0.2
type input =
Axis of int * int * float * float * float ref (* (index, deadband, limit, exponent, trim ) *)
| Button of int
+ | Hat of int
(** Description of a message *)
type msg = {
@@ -207,6 +208,7 @@ let parse_input = fun input ->
let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
Axis (index, deadband, limit, exponent, ref trim)
| "button" -> Button index
+ | "hat" -> Hat index
| _ -> failwith "parse_input: unexepcted tag" in
(name, value))
(Xml.children input)
@@ -290,6 +292,7 @@ let trim_set = fun inputs value ->
match input with
Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
| Button i -> failwith "No trim for buttons"
+ | Hat _ -> failwith "No trim for hats"
(** Input the trim file if it exists *)
@@ -341,10 +344,11 @@ let apply_trim = fun x trim ->
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 ->
+let eval_input = fun buttons hat axis input ->
match input with
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
+ | Hat _ -> hat
(** Scale a value in the given bounds *)
let scale = fun x min max ->
@@ -371,7 +375,7 @@ let pprz_mode = fun mode ->
else 1
(** Eval a function call (TO BE COMPLETED) *)
-let eval_call = fun f args ->
+let eval_call = fun f args hat->
match f, args with
"-", [a] -> - a
| "-", [a1; a2] -> a1 - a2
@@ -382,6 +386,15 @@ let eval_call = fun f args ->
| "||", [a1; a2] -> a1 lor a2
| "<", [a1; a2] -> if a1 < a2 then 1 else 0
| ">", [a1; a2] -> if a1 > a2 then 1 else 0
+ | "HatCentered", [_] -> if hat = 0 then 1 else 0
+ | "HatUp", [_] -> if hat = 1 then 1 else 0
+ | "HatRight", [_] -> if hat = 2 then 1 else 0
+ | "HatRightUp", [_] -> if hat = 3 then 1 else 0
+ | "HatDown", [_] -> if hat = 4 then 1 else 0
+ | "HatRightDown", [_] -> if hat = 6 then 1 else 0
+ | "HatLeft", [_] -> if hat = 8 then 1 else 0
+ | "HatLeftUp", [_] -> if hat = 9 then 1 else 0
+ | "HatLeftDown", [_] -> if hat = 12 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)
@@ -390,22 +403,22 @@ let eval_call = fun f args ->
| f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
(** Eval an expression *)
-let eval_expr = fun buttons axis inputs variables expr ->
+let eval_expr = fun buttons hat axis inputs variables expr ->
let rec eval = function
- Syntax.Ident ident ->
- (* 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)
+ | Syntax.Ident ident ->
+ (* try input first, then variables and hat position *)
+ let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
+ | (true, _) -> eval_input buttons hat axis (List.assoc ident inputs)
| (false, true) ->
- let v = List.assoc ident variables in
- v.value
+ let v = List.assoc ident variables in
+ v.value
| (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
- in
- i
+ 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) hat
| Syntax.Index _ -> failwith "eval_expr: index"
| Syntax.Field _ -> failwith "eval_expr: Field"
| Syntax.Deref _ -> failwith "eval_expr: deref" in
@@ -433,6 +446,7 @@ let trim_save_add_leaf = fun x channel_pair ->
match channel with
Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf "" chan_name trim.contents)
| Button i -> Printf.printf "%d" i
+ | Hat _ -> Printf.printf "hat"
(** save trim settings to file *)
let trim_save = fun inputs ->
@@ -450,13 +464,14 @@ 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"
+ | Button _ -> failwith "No trim for buttons"
+ | Hat _ -> failwith "No trim for hats"
(** Update variables state *)
-let update_variables = fun inputs buttons axis variables ->
+let update_variables = fun inputs buttons hat axis variables ->
List.iter (fun (_,var) ->
List.iter (fun (value, expr) ->
- let event = eval_expr buttons axis inputs variables expr in
+ let event = eval_expr buttons hat axis inputs variables expr in
if event <> 0 then begin
(* remove and add again ? *)
var.value <- value
@@ -465,16 +480,16 @@ let update_variables = fun inputs buttons axis variables ->
) 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 execute_action = fun ac_id inputs buttons hat axis variables message ->
let values =
List.map
- (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs variables expr)))
+ (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons hat axis inputs variables expr)))
message.fields
and on_event =
match message.on_event with
None -> true
- | Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
+ | Some expr -> eval_expr buttons hat axis inputs variables expr <> 0 in
let previous_values = get_previous_values message.msg_name in
(* FIXME ((value <> previous) && on_event) || send_always ??? *)
@@ -492,14 +507,15 @@ let execute_action = fun ac_id inputs buttons axis variables message ->
(** Output on stderr the values from the input device *)
-let print_inputs = fun nb_buttons buttons axis ->
+let print_inputs = fun nb_buttons buttons hat axis ->
print_string "buttons: ";
for i = 0 to nb_buttons - 1 do
- printf "%d:%d " i (eval_input buttons axis (Button i))
+ printf "%d:%d " i (eval_input buttons hat axis (Button i))
done;
+ printf "\nhat: %d" (eval_input buttons hat axis (Hat 0));
print_string "\naxis: ";
for i = 0 to Array.length axis - 1 do
- printf "%d:%d " i (eval_input buttons axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
+ printf "%d:%d " i (eval_input buttons hat axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
done;
ignore (print_newline ())
@@ -508,14 +524,14 @@ let print_inputs = fun nb_buttons buttons axis ->
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
+ let (nb_buttons, buttons, hat, axis) = stick_read () in
if !verbose then
- print_inputs nb_buttons buttons axis;
+ print_inputs nb_buttons buttons hat axis;
(* 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
+ update_variables actions.inputs buttons hat axis actions.variables;
+ List.iter (execute_action ac_id actions.inputs buttons hat axis actions.variables) actions.messages
with
exc -> prerr_endline (Printexc.to_string exc)
diff --git a/sw/ground_segment/joystick/ml_sdl_stick.c b/sw/ground_segment/joystick/ml_sdl_stick.c
index 08b0aee696..6b76f8f0d8 100644
--- a/sw/ground_segment/joystick/ml_sdl_stick.c
+++ b/sw/ground_segment/joystick/ml_sdl_stick.c
@@ -48,9 +48,10 @@ ml_stick_init(value device_index_val) {
return Val_int(opened);
}
-/** Return a triple of
+/** Return a tuple with
* - the number of buttons
* - one integer with the buttons values
+ * - one integer with the hat value
* - the array of axis values
*
* @param _unit placeholder for ocaml equivalent of void
@@ -58,20 +59,22 @@ ml_stick_init(value device_index_val) {
CAMLprim value
ml_stick_read(value _unit) {
CAMLparam0();
- CAMLlocal3 (result, buttons, axis);
+ CAMLlocal4 (result, buttons, hat, axis);
stick_read();
buttons = Val_int(stick_button_values);
+ hat = Val_int(stick_hat_value);
axis = caml_alloc_tuple(stick_axis_count);
unsigned int i;
for(i = 0; i < stick_axis_count; i++)
Store_field(axis, i, Val_int(stick_axis_values[i]));
- result = caml_alloc_tuple(3);
+ result = caml_alloc_tuple(4);
Store_field(result, 0, Val_int(stick_button_count));
Store_field(result, 1, buttons);
- Store_field(result, 2, axis);
+ Store_field(result, 2, hat);
+ Store_field(result, 3, axis);
CAMLreturn(result);
}
diff --git a/sw/ground_segment/joystick/sdl_stick.c b/sw/ground_segment/joystick/sdl_stick.c
index 9218e69116..ff2a68359e 100644
--- a/sw/ground_segment/joystick/sdl_stick.c
+++ b/sw/ground_segment/joystick/sdl_stick.c
@@ -50,6 +50,7 @@ SDL_Joystick *sdl_joystick;
SDL_Event sdl_event;
int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
+uint8_t stick_hat_value = 0;
int32_t stick_button_values = 0;
int stick_axis_count = 0;
@@ -84,6 +85,13 @@ int init_sdl_device(int device_index)
dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
}
+ /* How many POV hats available */
+ int stick_hat_count = SDL_JoystickNumHats(sdl_joystick);
+ dbgprintf(stderr,"Available hats: %d (0x%x)\n",stick_hat_count,stick_hat_count);
+ if (stick_hat_count > 1) {
+ dbgprintf(stderr,"ERROR: only one POV hat supported [%s:%d]\n",__FILE__,__LINE__);
+ }
+
/* How many axes available */
stick_axis_count = min(SDL_JoystickNumAxes(sdl_joystick),AXIS_COUNT);
dbgprintf(stderr,"Available axes: %d (0x%x)\n",stick_axis_count,stick_axis_count);
@@ -123,9 +131,8 @@ int stick_read( void ) {
switch(sdl_event.type)
{
case SDL_JOYBUTTONDOWN:
- //falls through to JOYBUTTONUP
+ //falls through to JOYBUTTONUP
case SDL_JOYBUTTONUP:
- {
for (cnt = 0; cnt < stick_button_count; cnt++) {
if (sdl_event.jbutton.button == cnt) {
if (sdl_event.jbutton.state == SDL_PRESSED) {
@@ -134,8 +141,15 @@ int stick_read( void ) {
break;
}
}
- }
- break;
+ break;
+
+ case SDL_JOYHATMOTION:
+ // only one hat (with index 0) supported
+ if (sdl_event.jhat.hat == 0) {
+ stick_hat_value = sdl_event.jhat.value;
+ break;
+ }
+ break;
case SDL_JOYAXISMOTION:
for (cnt = 0; cnt < stick_axis_count; cnt++) {
@@ -144,31 +158,34 @@ int stick_read( void ) {
break;
}
}
- break;
+ break;
case SDL_QUIT:
- printf("Quitting...\n");
- exit(1);
- break;
+ printf("Quitting...\n");
+ exit(1);
+ break;
default:
- //do nothing
- //printf("unknown SDL event!!!\n");
- break;
+ //do nothing
+ //printf("unknown SDL event!!!\n");
+ break;
}
}
- dbgprintf(stderr,"buttons ");
+ dbgprintf(stderr, "buttons ");
for (cnt = 0; cnt < stick_button_count; cnt++) {
- dbgprintf(stderr,"%d ",(stick_button_values >> cnt) & 1 );
+ dbgprintf(stderr, "%d ", (stick_button_values >> cnt) & 1 );
}
- dbgprintf(stderr,"| axes ");
+ dbgprintf(stderr, "| hat ");
+ dbgprintf(stderr, "%d ", stick_hat_value);
+
+ dbgprintf(stderr, "| axes ");
for (cnt = 0; cnt < stick_axis_count; cnt++) {
- dbgprintf(stderr,"%d ",stick_axis_values[cnt]);
+ dbgprintf(stderr, "%d ", stick_axis_values[cnt]);
}
- dbgprintf(stderr,"\n");
+ dbgprintf(stderr, "\n");
return 0;
}
diff --git a/sw/ground_segment/joystick/sdl_stick.h b/sw/ground_segment/joystick/sdl_stick.h
index 6e5f52825d..28ecaa0a7f 100644
--- a/sw/ground_segment/joystick/sdl_stick.h
+++ b/sw/ground_segment/joystick/sdl_stick.h
@@ -40,6 +40,7 @@
/* Global variables about the initialized device */
extern int8_t stick_axis_values[STICK_AXIS_COUNT];
extern int32_t stick_button_values;
+extern uint8_t stick_hat_value;
extern int stick_axis_count, stick_button_count;
/** Initialize a joystick with SDL.
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index 52903850ed..f87e000747 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -67,10 +67,12 @@ void nps_autopilot_run_systime_step( void ) {
void nps_autopilot_run_step(double time __attribute__ ((unused))) {
+#ifdef RADIO_CONTROL_TYPE_PPM
if (nps_radio_control_available(time)) {
radio_control_feed();
main_event();
}
+#endif
if (nps_sensors_gyro_available()) {
imu_feed_gyro_accel();
diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c
index dda8020836..93847c425c 100644
--- a/sw/simulator/nps/nps_ivy.c
+++ b/sw/simulator/nps/nps_ivy.c
@@ -12,6 +12,10 @@
#include "subsystems/ins.h"
#include "firmwares/rotorcraft/navigation.h"
+#ifdef RADIO_CONTROL_TYPE_DATALINK
+#include "subsystems/radio_control.h"
+#endif
+
#include NPS_SENSORS_PARAMS
@@ -36,6 +40,16 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
+#ifdef RADIO_CONTROL_TYPE_DATALINK
+static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]);
+
+static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]);
+#endif
+
void nps_ivy_init(char* ivy_bus) {
const char* agent_name = AIRFRAME_NAME"_NPS";
const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
@@ -46,6 +60,11 @@ void nps_ivy_init(char* ivy_bus) {
IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)");
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+#ifdef RADIO_CONTROL_TYPE_DATALINK
+ IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)");
+ IvyBindMsg(on_DL_RC_4CH, NULL, "^(\\S*) RC_4CH (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+#endif
+
#ifdef __APPLE__
const char* default_ivy_bus = "224.255.255.255";
#else
@@ -117,6 +136,30 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
}
+#ifdef RADIO_CONTROL_TYPE_DATALINK
+static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]){
+ uint8_t throttle_mode = atoi(argv[2]);
+ int8_t roll = atoi(argv[3]);
+ int8_t pitch = atoi(argv[4]);
+ parse_rc_3ch_datalink(throttle_mode, roll, pitch);
+ //printf("rc_3ch: throttle_mode %d, roll %d, pitch %d\n", throttle_mode, roll, pitch);
+}
+
+static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]){
+ uint8_t mode = atoi(argv[2]);
+ uint8_t throttle = atoi(argv[3]);
+ int8_t roll = atoi(argv[4]);
+ int8_t pitch = atoi(argv[5]);
+ int8_t yaw = atoi(argv[6]);
+ parse_rc_4ch_datalink(mode, throttle, roll, pitch, yaw);
+ //printf("rc_4ch: mode %d, throttle %d, roll %d, pitch %d, yaw %d\n", mode, throttle, roll, pitch, yaw);
+}
+#endif
+
void nps_ivy_display(void) {
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
diff --git a/sw/tools/dfu/stm32_mem.py b/sw/tools/dfu/stm32_mem.py
index 9e23af6b02..d6da3e6831 100644
--- a/sw/tools/dfu/stm32_mem.py
+++ b/sw/tools/dfu/stm32_mem.py
@@ -108,6 +108,8 @@ if __name__ == "__main__":
print('.', end="")
stdout.flush()
time.sleep(0.5)
+ else:
+ break
print("")
if not devs:
print("No DFU devices found!")