Merge pull request #1541 from paparazzi/refactor_xsens

Refactor xsens

Factor out the common code and convert the subsystems to modules.
This is NOT tested with real hardware so far (not many people have a XSens).

- `XSENS_OUTPUT_MODE` might be wrong (or set it to output more than needed) in some cases (like for pure IMU mode)
- The different signs (IMU/sensor orientation) between the different xsens types is also weird and unclear.
See also related #924
This commit is contained in:
Felix Ruess
2016-03-30 23:13:55 +02:00
25 changed files with 1599 additions and 1505 deletions
+3 -3
View File
@@ -216,9 +216,9 @@
<configure name="MODEM_BAUD" value="B9600"/>
</subsystem>
<subsystem name="ins" type="xsens">
<configure name="XSENS_UART_NR" value="1"/>
</subsystem>
<module name="ins" type="xsens">
<configure name="XSENS_PORT" value="uart1"/>
</module>
</firmware>
+8 -8
View File
@@ -36,10 +36,10 @@
<configure name="MODEM_BAUD" value="B9600"/>
<configure name="MODEM_PORT" value="UART3"/>
</subsystem>
<subsystem name="ins" type="xsens700">
<configure name="XSENS_UART_NR" value="1"/>
<configure name="XSENS_UART_BAUD" value="B230400"/>
</subsystem>
<module name="ins" type="xsens700">
<configure name="XSENS_PORT" value="uart1"/>
<configure name="XSENS_BAUD" value="B230400"/>
</module>
</target>
<target name="fbw" board="lisa_m_2.0">
<configure name="SEPARATE_FBW" value="1"/>
@@ -70,10 +70,10 @@
<configure name="MODEM_BAUD" value="B9600"/>
<configure name="MODEM_PORT" value="UART3"/>
</subsystem>
<subsystem name="ins" type="xsens700">
<configure name="XSENS_UART_NR" value="1"/>
<configure name="XSENS_UART_BAUD" value="B230400"/>
</subsystem>
<module name="ins" type="xsens700">
<configure name="XSENS_PORT" value="uart1"/>
<configure name="XSENS_BAUD" value="B230400"/>
</module>
</target>
<define name="AGR_CLIMB"/>
<define name="LOITER_TRIM"/>
+4 -4
View File
@@ -25,10 +25,10 @@
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<subsystem name="ins" type="xsens">
<configure name="XSENS_UART_NR" value="0"/>
<configure name="XSENS_UART_BAUD" value="B230400"/>
</subsystem>
<module name="ins" type="xsens">
<configure name="XSENS_PORT" value="uart0"/>
<configure name="XSENS_BAUD" value="B230400"/>
</module>
</firmware>
<!-- ************************* MODULES ************************* -->
@@ -21,10 +21,10 @@
<subsystem name="telemetry" type="transparent"/>
<!-- Sensors -->
<subsystem name="imu" type="xsens">
<configure name="XSENS_UART_NR" value="3"/>
<configure name="XSENS_UART_BAUD" value="B115200"/>
</subsystem>
<module name="imu" type="xsens">
<configure name="XSENS_PORT" value="uart3"/>
<configure name="XSENS_BAUD" value="B115200"/>
</module>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="control"/>
@@ -21,10 +21,10 @@
<subsystem name="telemetry" type="transparent"/>
<!-- Sensors -->
<subsystem name="ins" type="xsens">
<configure name="XSENS_UART_NR" value="3"/>
<configure name="XSENS_UART_BAUD" value="B115200"/>
</subsystem>
<module name="ins" type="xsens">
<configure name="XSENS_PORT" value="uart3"/>
<configure name="XSENS_BAUD" value="B115200"/>
</module>
<subsystem name="control"/>
<subsystem name="navigation"/>
@@ -1,68 +1 @@
# Hey Emacs, this is a -*- makefile -*-
# XSens Mti just providing IMU measurements
# <subsystem name="imu" type="xsens">
# <configure name="XSENS_UART_NR" value="0"/>
# <configure name="XSENS_UART_BAUD" value="B115200"/>
# </subsystem>
#
# <section name="IMU" prefix="IMU_">
# <define name="GYRO_P_SIGN" value="1"/>
# <define name="GYRO_Q_SIGN" value="1"/>
# <define name="GYRO_R_SIGN" value="1"/>
#
# <define name="GYRO_P_NEUTRAL" value="0"/>
# <define name="GYRO_R_NEUTRAL" value="0"/>
# <define name="GYRO_Q_NEUTRAL" value="0"/>
#
# <define name="GYRO_P_SENS" value="1" integer="16"/>
# <define name="GYRO_R_SENS" value="1" integer="16"/>
# <define name="GYRO_Q_SENS" value="1" integer="16"/>
#
# <define name="ACCEL_X_SIGN" value="1"/>
# <define name="ACCEL_Y_SIGN" value="1"/>
# <define name="ACCEL_Z_SIGN" value="1"/>
#
# <define name="ACCEL_X_SENS" value="1" integer="16"/>
# <define name="ACCEL_Z_SENS" value="1" integer="16"/>
# <define name="ACCEL_Y_SENS" value="1" integer="16"/>
#
# <define name="ACCEL_X_NEUTRAL" value="0"/>
# <define name="ACCEL_Z_NEUTRAL" value="0"/>
# <define name="ACCEL_Y_NEUTRAL" value="0"/>
#
# <define name="MAG_X_SIGN" value="1"/>
# <define name="MAG_Y_SIGN" value="1"/>
# <define name="MAG_Z_SIGN" value="1"/>
#
# <define name="MAG_X_NEUTRAL" value="-45"/>
# <define name="MAG_Y_NEUTRAL" value="334"/>
# <define name="MAG_Z_NEUTRAL" value="7"/>
#
# <define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
# <define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
# <define name="MAG_Z_SENS" value="4.41585354498" integer="16"/>
#
# <define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
# <define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
# <define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
# </section>
#########################################
## IMU
ap.CFLAGS += -DUSE_IMU
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c
ap.srcs += $(SRC_SUBSYSTEMS)/imu.c
ifndef XSENS_UART_BAUD
XSENS_UART_BAUD = B115200
endif
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR)
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
$(error Error: The imu xsens subsystem has been converted to a module, replace <subsystem name="imu" type="xsens"/> by <module name="imu" type="xsens"/>)
@@ -1,70 +1 @@
# Hey Emacs, this is a -*- makefile -*-
# XSens Mti-G
# <subsystem name="ins" type="xsens">
# <configure name="XSENS_UART_NR" value="0"/>
# <configure name="XSENS_UART_BAUD" value="B115200"/>
# </subsystem>
#########################################
## ATTITUDE
ap.CFLAGS += -DUSE_INS_MODULE
# AHRS Results
ap.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_xsens.h\"
ifndef XSENS_UART_BAUD
XSENS_UART_BAUD = B115200
endif
#B230400
#B115200
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR)
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c
ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
#########################################
## GPS
ap.CFLAGS += -DUSE_GPS_XSENS
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
ap.CFLAGS += -DGPS_NB_CHANNELS=16
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
#########################################
## Simulator
SIM_TARGETS = sim nps
ifneq (,$(findstring $(TARGET),$(SIM_TARGETS)))
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
$(TARGET).CFLAGS += -DUSE_AHRS
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
$(TARGET).CFLAGS += -DUSE_GPS
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
endif
$(error Error: The ins xsens subsystem has been converted to a module, replace <subsystem name="ins" type="xsens"/> by <module name="ins" type="xsens"/>)
@@ -1,64 +1 @@
# Hey Emacs, this is a -*- makefile -*-
# XSens Mti-G
# <subsystem name="ins" type="xsens700">
# <configure name="XSENS_UART_NR" value="0"/>
# <configure name="XSENS_UART_BAUD" value="B115200"/>
# </subsystem>
#########################################
## ATTITUDE
ap.CFLAGS += -DUSE_INS_MODULE
# AHRS Results
ap.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_xsens.h\"
ifndef XSENS_UART_BAUD
XSENS_UART_BAUD = B115200
endif
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR)
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
ap.srcs += $(SRC_MODULES)/ins/ins_xsens700.c
ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
#########################################
## GPS
ap.CFLAGS += -DUSE_GPS_XSENS
ap.CFLAGS += -DGPS_NB_CHANNELS=50
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
#########################################
## Simulator
SIM_TARGETS = sim nps
ifneq (,$(findstring $(TARGET),$(SIM_TARGETS)))
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
$(TARGET).CFLAGS += -DUSE_AHRS
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
$(TARGET).CFLAGS += -DUSE_GPS
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
endif
$(error Error: The ins xsens700 subsystem has been converted to a module, replace <subsystem name="ins" type="xsens700"/> by <module name="ins" type="xsens700"/>)
+49
View File
@@ -0,0 +1,49 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins_xsens700" dir="ins">
<doc>
<description>
XSens IMU
</description>
<configure name="XSENS_PORT" value="uart1" description="The (uart) port the XSens is connected to"/>
<configure name="XSENS_BAUD" value="B115200" description="UART baud rate"/>
</doc>
<header>
<file name="imu_xsens.h"/>
</header>
<!-- imu init/periodic/event still called explicitly in main -->
<makefile target="ap">
<file name="xsens.c"/>
<file name="xsens_common.c"/>
<file name="imu_xsens.c"/>
<file name="imu.c" dir="subsystems"/>
<configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
<configure name="XSENS_BAUD" default="B115200"/>
<define name="USE_$(XSENS_PORT_UPPER)" value="1"/>
<define name="XSENS_LINK" value="$(XSENS_PORT_LOWER)"/>
<define name="$(XSENS_PORT_UPPER)_BAUD" value="$(XSENS_BAUD)"/>
<!-- TODO: check output mode -->
<define name="XSENS_OUTPUT_MODE" value="0x1836"/>
<raw>
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/imu_xsens.h\"
</raw>
</makefile>
<makefile target="sim">
<file name="ahrs.c" dir="subsystems"/>
<file name="ahrs_sim.c" dir="subsystems/ahrs"/>
<define name="USE_AHRS"/>
<raw>
sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
</raw>
</makefile>
<makefile target="nps">
<define name="USE_IMU"/>
<file name="imu_nps.c" dir="subsystems/imu"/>
<raw>
nps.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_nps.h\"
</raw>
</makefile>
</module>
+67 -6
View File
@@ -1,16 +1,77 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins">
<module name="ins_xsens" dir="ins">
<doc>
<description>XSens</description>
<description>
XSens Mti-G INS
</description>
<configure name="XSENS_PORT" value="uart1" description="The (uart) port the XSens is connected to"/>
<configure name="XSENS_BAUD" value="B115200" description="UART baud rate"/>
</doc>
<header>
<file name="ins_module.h"/>
<file name="ins_xsens.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic()" freq="60"/>
<makefile>
<!-- ins_init is still called explicitly in main -->
<!--init fun="ins_init()"/-->
<periodic fun="xsens_periodic()" freq="60"/>
<event fun="ins_xsens_event()"/>
<makefile target="ap">
<file name="xsens.c"/>
<file name="xsens_common.c"/>
<file name="ins_xsens.c"/>
<file name="ins.c" dir="subsystems"/>
<configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
<configure name="XSENS_BAUD" default="B115200"/>
<define name="USE_$(XSENS_PORT_UPPER)" value="1"/>
<define name="XSENS_LINK" value="$(XSENS_PORT_LOWER)"/>
<define name="$(XSENS_PORT_UPPER)_BAUD" value="$(XSENS_BAUD)"/>
<define name="XSENS_OUTPUT_MODE" value="0x1836"/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
<file name="gps.c" dir="subsystems"/>
<define name="USE_GPS"/>
<define name="USE_GPS_XSENS"/>
<define name="USE_GPS_XSENS_RAW_DATA"/>
<define name="GPS_NB_CHANNELS" value="16"/>
<raw>
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
</raw>
</makefile>
<makefile target="sim">
<file name="ahrs.c" dir="subsystems"/>
<file name="ahrs_sim.c" dir="subsystems/ahrs"/>
<define name="USE_AHRS"/>
<file name="ins.c" dir="subsystems"/>
<file name="ins_gps_passthrough_utm.c" dir="subsystems/ins"/>
<file name="gps.c" dir="subsystems"/>
<file name="gps_sim.c" dir="subsystems/gps"/>
<define name="USE_GPS"/>
<raw>
sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
sim.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
</raw>
</makefile>
<makefile target="nps">
<!-- nps dummy ahrs missing -->
<file name="ahrs.c" dir="subsystems"/>
<define name="USE_AHRS"/>
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
<file name="ins.c" dir="subsystems"/>
<file name="ins_gps_passthrough_utm.c" dir="subsystems/ins"/>
<file name="gps.c" dir="subsystems"/>
<file name="gps_sim_nps.c" dir="subsystems/gps"/>
<define name="USE_GPS"/>
<raw>
nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
</raw>
</makefile>
</module>
+76
View File
@@ -0,0 +1,76 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins_xsens700" dir="ins">
<doc>
<description>
XSens Mti-G 700 INS
</description>
<configure name="XSENS_PORT" value="uart1" description="The (uart) port the XSens is connected to"/>
<configure name="XSENS_BAUD" value="B115200" description="UART baud rate"/>
</doc>
<header>
<file name="ins_xsens700.h"/>
</header>
<!-- ins_init is still called explicitly in main -->
<!--init fun="ins_init()"/-->
<periodic fun="xsens700_periodic()" freq="60"/>
<event fun="ins_xsens700_event()"/>
<makefile target="ap">
<file name="xsens700.c"/>
<file name="xsens_common.c"/>
<file name="ins_xsens700.c"/>
<file name="ins.c" dir="subsystems"/>
<configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
<configure name="XSENS_BAUD" default="B115200"/>
<define name="USE_$(XSENS_PORT_UPPER)" value="1"/>
<define name="XSENS_LINK" value="$(XSENS_PORT_LOWER)"/>
<define name="$(XSENS_PORT_UPPER)_BAUD" value="$(XSENS_BAUD)"/>
<define name="XSENS_OUTPUT_MODE" value="0x1836"/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
<file name="gps.c" dir="subsystems"/>
<define name="USE_GPS"/>
<define name="USE_GPS_XSENS"/>
<define name="GPS_NB_CHANNELS" value="50"/>
<raw>
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens700.h\"
</raw>
</makefile>
<makefile target="sim">
<file name="ahrs.c" dir="subsystems"/>
<file name="ahrs_sim.c" dir="subsystems/ahrs"/>
<define name="USE_AHRS"/>
<file name="ins.c" dir="subsystems"/>
<file name="ins_gps_passthrough_utm.c" dir="subsystems/ins"/>
<file name="gps.c" dir="subsystems"/>
<file name="gps_sim.c" dir="subsystems/gps"/>
<define name="USE_GPS"/>
<raw>
sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
sim.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
</raw>
</makefile>
<makefile target="nps">
<!-- nps dummy ahrs missing -->
<file name="ahrs.c" dir="subsystems"/>
<define name="USE_AHRS"/>
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
<file name="ins.c" dir="subsystems"/>
<file name="ins_gps_passthrough_utm.c" dir="subsystems/ins"/>
<file name="gps.c" dir="subsystems"/>
<file name="gps_sim_nps.c" dir="subsystems/gps"/>
<define name="USE_GPS"/>
<raw>
nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
</raw>
</makefile>
</module>
-26
View File
@@ -1,26 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins">
<doc>
<description>XSens MTiG</description>
</doc>
<!-- <conflicts>ins</conflicts> -->
<!-- <requires>gps_xsens</requires> -->
<header>
<file name="ins_module.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic()" freq="60"/>
<makefile>
<define name="USE_UART0" value="1"/>
<define name="INS_LINK" value="UART0"/>
<define name="UART0_BAUD" value="B115200"/>
<define name="USE_GPS_XSENS"/>
<!-- calibrated, orientation, position, velocity, status -->
<define name="XSENS_OUTPUT_MODE" value="0x0836"/>
<!-- timestamp, euler, acc, rate, mag, float, no aux, NED -->
<define name="XSENS_OUTPUT_SETTINGS" value="0x80000C05"/>
<file name="ins_xsens.c"/>
</makefile>
</module>
-24
View File
@@ -1,24 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins">
<doc>
<description>XSens MTi</description>
</doc>
<!-- <conflicts>ins</conflicts> -->
<header>
<file name="ins_module.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic()" freq="60"/>
<makefile>
<define name="USE_UART0" value="1"/>
<define name="INS_LINK" value="UART0"/>
<define name="UART0_BAUD" value="B115200"/>
<!-- calibrated, orientation, status -->
<define name="XSENS_OUTPUT_MODE" value="0x0806"/>
<!-- timestamp, euler, acc, rate, mag, float, no aux, NED -->
<define name="XSENS_OUTPUT_SETTINGS" value="0x80000C05"/>
<file name="ins_xsens.c"/>
</makefile>
</module>