mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 20:38:27 +08:00
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
@@ -216,9 +216,9 @@
|
|||||||
<configure name="MODEM_BAUD" value="B9600"/>
|
<configure name="MODEM_BAUD" value="B9600"/>
|
||||||
</subsystem>
|
</subsystem>
|
||||||
|
|
||||||
<subsystem name="ins" type="xsens">
|
<module name="ins" type="xsens">
|
||||||
<configure name="XSENS_UART_NR" value="1"/>
|
<configure name="XSENS_PORT" value="uart1"/>
|
||||||
</subsystem>
|
</module>
|
||||||
|
|
||||||
</firmware>
|
</firmware>
|
||||||
|
|
||||||
|
|||||||
@@ -36,10 +36,10 @@
|
|||||||
<configure name="MODEM_BAUD" value="B9600"/>
|
<configure name="MODEM_BAUD" value="B9600"/>
|
||||||
<configure name="MODEM_PORT" value="UART3"/>
|
<configure name="MODEM_PORT" value="UART3"/>
|
||||||
</subsystem>
|
</subsystem>
|
||||||
<subsystem name="ins" type="xsens700">
|
<module name="ins" type="xsens700">
|
||||||
<configure name="XSENS_UART_NR" value="1"/>
|
<configure name="XSENS_PORT" value="uart1"/>
|
||||||
<configure name="XSENS_UART_BAUD" value="B230400"/>
|
<configure name="XSENS_BAUD" value="B230400"/>
|
||||||
</subsystem>
|
</module>
|
||||||
</target>
|
</target>
|
||||||
<target name="fbw" board="lisa_m_2.0">
|
<target name="fbw" board="lisa_m_2.0">
|
||||||
<configure name="SEPARATE_FBW" value="1"/>
|
<configure name="SEPARATE_FBW" value="1"/>
|
||||||
@@ -70,10 +70,10 @@
|
|||||||
<configure name="MODEM_BAUD" value="B9600"/>
|
<configure name="MODEM_BAUD" value="B9600"/>
|
||||||
<configure name="MODEM_PORT" value="UART3"/>
|
<configure name="MODEM_PORT" value="UART3"/>
|
||||||
</subsystem>
|
</subsystem>
|
||||||
<subsystem name="ins" type="xsens700">
|
<module name="ins" type="xsens700">
|
||||||
<configure name="XSENS_UART_NR" value="1"/>
|
<configure name="XSENS_PORT" value="uart1"/>
|
||||||
<configure name="XSENS_UART_BAUD" value="B230400"/>
|
<configure name="XSENS_BAUD" value="B230400"/>
|
||||||
</subsystem>
|
</module>
|
||||||
</target>
|
</target>
|
||||||
<define name="AGR_CLIMB"/>
|
<define name="AGR_CLIMB"/>
|
||||||
<define name="LOITER_TRIM"/>
|
<define name="LOITER_TRIM"/>
|
||||||
|
|||||||
@@ -25,10 +25,10 @@
|
|||||||
<subsystem name="control"/>
|
<subsystem name="control"/>
|
||||||
<!-- Sensors -->
|
<!-- Sensors -->
|
||||||
<subsystem name="navigation"/>
|
<subsystem name="navigation"/>
|
||||||
<subsystem name="ins" type="xsens">
|
<module name="ins" type="xsens">
|
||||||
<configure name="XSENS_UART_NR" value="0"/>
|
<configure name="XSENS_PORT" value="uart0"/>
|
||||||
<configure name="XSENS_UART_BAUD" value="B230400"/>
|
<configure name="XSENS_BAUD" value="B230400"/>
|
||||||
</subsystem>
|
</module>
|
||||||
</firmware>
|
</firmware>
|
||||||
|
|
||||||
<!-- ************************* MODULES ************************* -->
|
<!-- ************************* MODULES ************************* -->
|
||||||
|
|||||||
@@ -21,10 +21,10 @@
|
|||||||
<subsystem name="telemetry" type="transparent"/>
|
<subsystem name="telemetry" type="transparent"/>
|
||||||
|
|
||||||
<!-- Sensors -->
|
<!-- Sensors -->
|
||||||
<subsystem name="imu" type="xsens">
|
<module name="imu" type="xsens">
|
||||||
<configure name="XSENS_UART_NR" value="3"/>
|
<configure name="XSENS_PORT" value="uart3"/>
|
||||||
<configure name="XSENS_UART_BAUD" value="B115200"/>
|
<configure name="XSENS_BAUD" value="B115200"/>
|
||||||
</subsystem>
|
</module>
|
||||||
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||||
<subsystem name="gps" type="ublox"/>
|
<subsystem name="gps" type="ublox"/>
|
||||||
<subsystem name="control"/>
|
<subsystem name="control"/>
|
||||||
|
|||||||
@@ -21,10 +21,10 @@
|
|||||||
<subsystem name="telemetry" type="transparent"/>
|
<subsystem name="telemetry" type="transparent"/>
|
||||||
|
|
||||||
<!-- Sensors -->
|
<!-- Sensors -->
|
||||||
<subsystem name="ins" type="xsens">
|
<module name="ins" type="xsens">
|
||||||
<configure name="XSENS_UART_NR" value="3"/>
|
<configure name="XSENS_PORT" value="uart3"/>
|
||||||
<configure name="XSENS_UART_BAUD" value="B115200"/>
|
<configure name="XSENS_BAUD" value="B115200"/>
|
||||||
</subsystem>
|
</module>
|
||||||
|
|
||||||
<subsystem name="control"/>
|
<subsystem name="control"/>
|
||||||
<subsystem name="navigation"/>
|
<subsystem name="navigation"/>
|
||||||
|
|||||||
@@ -1,68 +1 @@
|
|||||||
# Hey Emacs, this is a -*- makefile -*-
|
$(error Error: The imu xsens subsystem has been converted to a module, replace <subsystem name="imu" type="xsens"/> by <module name="imu" type="xsens"/>)
|
||||||
|
|
||||||
# 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
|
|
||||||
|
|||||||
@@ -1,70 +1 @@
|
|||||||
# Hey Emacs, this is a -*- makefile -*-
|
$(error Error: The ins xsens subsystem has been converted to a module, replace <subsystem name="ins" type="xsens"/> by <module name="ins" type="xsens"/>)
|
||||||
|
|
||||||
# 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
|
|
||||||
|
|
||||||
|
|||||||
@@ -1,64 +1 @@
|
|||||||
# Hey Emacs, this is a -*- makefile -*-
|
$(error Error: The ins xsens700 subsystem has been converted to a module, replace <subsystem name="ins" type="xsens700"/> by <module name="ins" type="xsens700"/>)
|
||||||
|
|
||||||
# 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
|
|
||||||
|
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|
||||||
@@ -1,16 +1,77 @@
|
|||||||
<!DOCTYPE module SYSTEM "module.dtd">
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
<module name="ins">
|
<module name="ins_xsens" dir="ins">
|
||||||
<doc>
|
<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>
|
</doc>
|
||||||
<header>
|
<header>
|
||||||
<file name="ins_module.h"/>
|
<file name="ins_xsens.h"/>
|
||||||
</header>
|
</header>
|
||||||
<init fun="ins_init()"/>
|
<!-- ins_init is still called explicitly in main -->
|
||||||
<periodic fun="ins_periodic()" freq="60"/>
|
<!--init fun="ins_init()"/-->
|
||||||
<makefile>
|
<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_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>
|
</makefile>
|
||||||
</module>
|
</module>
|
||||||
|
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|
||||||
@@ -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>
|
|
||||||
|
|
||||||
@@ -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>
|
|
||||||
|
|
||||||
@@ -0,0 +1,96 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||||
|
*
|
||||||
|
* 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 imu_xsens.c
|
||||||
|
* XSENS to just provide IMU measurements.
|
||||||
|
* For use with an external AHRS algorithm.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "imu_xsens.h"
|
||||||
|
#include "xsens.h"
|
||||||
|
#include "xsens_common.h"
|
||||||
|
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
|
||||||
|
#include "mcu_periph/sys_time.h"
|
||||||
|
#include "subsystems/abi.h"
|
||||||
|
|
||||||
|
static void handle_ins_msg(void);
|
||||||
|
|
||||||
|
void imu_xsens_init(void)
|
||||||
|
{
|
||||||
|
xsens_init();
|
||||||
|
}
|
||||||
|
|
||||||
|
void imu_xsens_event(void)
|
||||||
|
{
|
||||||
|
xsens_event();
|
||||||
|
if (xsens.msg_received) {
|
||||||
|
parse_xsens_msg();
|
||||||
|
handle_ins_msg();
|
||||||
|
xsens.msg_received = FALSE;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void handle_ins_msg(void)
|
||||||
|
{
|
||||||
|
uint32_t now_ts = get_sys_time_usec();
|
||||||
|
#ifdef XSENS_BACKWARDS
|
||||||
|
if (xsens.gyro_available) {
|
||||||
|
RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(xsens.gyro.p), -RATE_BFP_OF_REAL(xsens.gyro.q), RATE_BFP_OF_REAL(xsens.gyro.r));
|
||||||
|
xsens.gyro_available = FALSE;
|
||||||
|
imu_scale_gyro(&imu);
|
||||||
|
AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro);
|
||||||
|
}
|
||||||
|
if (xsens.accel_available) {
|
||||||
|
VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(xsens.accel.ax), -ACCEL_BFP_OF_REAL(xsens.accel.ay), ACCEL_BFP_OF_REAL(xsens.accel.az));
|
||||||
|
xsens.accel_available = FALSE;
|
||||||
|
imu_scale_accel(&imu);
|
||||||
|
AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel);
|
||||||
|
}
|
||||||
|
if (xsens.mag_available) {
|
||||||
|
VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(xsens.mag.mx), -MAG_BFP_OF_REAL(xsens.mag.my), MAG_BFP_OF_REAL(xsens.mag.mz));
|
||||||
|
xsens.mag_available = FALSE;
|
||||||
|
imu_scale_mag(&imu);
|
||||||
|
AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag);
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
if (xsens.gyro_available) {
|
||||||
|
RATES_BFP_OF_REAL(imu.gyro_unscaled, xsens.gyro);
|
||||||
|
xsens.gyro_available = FALSE;
|
||||||
|
imu_scale_gyro(&imu);
|
||||||
|
AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro);
|
||||||
|
}
|
||||||
|
if (xsens.accel_available) {
|
||||||
|
ACCELS_BFP_OF_REAL(imu.accel_unscaled, xsens.accel);
|
||||||
|
xsens.accel_available = FALSE;
|
||||||
|
imu_scale_accel(&imu);
|
||||||
|
AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel);
|
||||||
|
}
|
||||||
|
if (xsens.mag_available) {
|
||||||
|
MAGS_BFP_OF_REAL(imu.mag_unscaled, xsens.mag);
|
||||||
|
xsens.mag_available = FALSE;
|
||||||
|
imu_scale_mag(&imu);
|
||||||
|
AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag);
|
||||||
|
}
|
||||||
|
#endif /* XSENS_BACKWARDS */
|
||||||
|
}
|
||||||
@@ -0,0 +1,45 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010 ENAC
|
||||||
|
*
|
||||||
|
* 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 modules/ins/imu_xsens.h
|
||||||
|
*
|
||||||
|
* XSENS to just provide IMU measurements.
|
||||||
|
* For use with an external AHRS algorithm.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef IMU_XSENS_H
|
||||||
|
#define IMU_XSENS_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
|
||||||
|
#include "subsystems/imu.h"
|
||||||
|
#include "xsens.h"
|
||||||
|
|
||||||
|
extern void imu_xsens_init(void);
|
||||||
|
extern void imu_xsens_event(void);
|
||||||
|
|
||||||
|
#define ImuEvent imu_xsens_event
|
||||||
|
#define imu_impl_init imu_xsens_init
|
||||||
|
#define imu_periodic xsens_periodic
|
||||||
|
|
||||||
|
#endif
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -21,7 +21,8 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* \brief Library for the XSENS AHRS
|
* @file modules/ins/ins_xsens.h
|
||||||
|
* Xsens as a full INS solution
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef INS_XSENS_H
|
#ifndef INS_XSENS_H
|
||||||
@@ -29,54 +30,22 @@
|
|||||||
|
|
||||||
#include "std.h"
|
#include "std.h"
|
||||||
|
|
||||||
#include "ins_module.h"
|
// hack to not use this in sim/nps
|
||||||
|
#ifndef SITL
|
||||||
|
#include "xsens.h"
|
||||||
|
|
||||||
struct XsensTime {
|
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
|
||||||
int8_t hour;
|
extern volatile uint8_t new_ins_attitude;
|
||||||
int8_t min;
|
|
||||||
int8_t sec;
|
|
||||||
int32_t nanosec;
|
|
||||||
int16_t year;
|
|
||||||
int8_t month;
|
|
||||||
int8_t day;
|
|
||||||
};
|
|
||||||
|
|
||||||
extern struct XsensTime xsens_time;
|
|
||||||
|
|
||||||
extern uint8_t xsens_msg_status;
|
|
||||||
extern uint16_t xsens_time_stamp;
|
|
||||||
|
|
||||||
extern void xsens_periodic(void);
|
|
||||||
|
|
||||||
/* To use Xsens to just provide IMU measurements
|
|
||||||
* for use with an external AHRS algorithm
|
|
||||||
*/
|
|
||||||
#if USE_IMU
|
|
||||||
#include "subsystems/imu.h"
|
|
||||||
#include "subsystems/abi.h"
|
|
||||||
|
|
||||||
struct ImuXsens {
|
|
||||||
bool_t gyro_available;
|
|
||||||
bool_t accel_available;
|
|
||||||
bool_t mag_available;
|
|
||||||
};
|
|
||||||
extern struct ImuXsens imu_xsens;
|
|
||||||
|
|
||||||
#define ImuEvent() {}
|
|
||||||
#endif /* USE_IMU */
|
|
||||||
|
|
||||||
|
|
||||||
/* use Xsens as a full INS solution */
|
|
||||||
#if USE_INS_MODULE
|
|
||||||
#define InsEvent() { \
|
|
||||||
ins_event_check_and_handle(handle_ins_msg); \
|
|
||||||
}
|
|
||||||
#define DefaultInsImpl ins_xsens
|
|
||||||
#define InsPeriodic xsens_periodic
|
|
||||||
extern void ins_xsens_init(void);
|
|
||||||
extern void ins_xsens_register(void);
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
extern float ins_pitch_neutral;
|
||||||
|
extern float ins_roll_neutral;
|
||||||
|
|
||||||
|
#define DefaultInsImpl ins_xsens
|
||||||
|
|
||||||
|
extern void ins_xsens_init(void);
|
||||||
|
extern void ins_xsens_register(void);
|
||||||
|
extern void ins_xsens_event(void);
|
||||||
|
|
||||||
#if USE_GPS_XSENS
|
#if USE_GPS_XSENS
|
||||||
#ifndef PRIMARY_GPS
|
#ifndef PRIMARY_GPS
|
||||||
@@ -86,4 +55,11 @@ extern void gps_xsens_init(void);
|
|||||||
extern void gps_xsens_register(void);
|
extern void gps_xsens_register(void);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#else // SITL
|
||||||
|
|
||||||
|
static inline void xsens_periodic(void) {}
|
||||||
|
static inline void ins_xsens_event(void) {}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,56 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010 ENAC
|
||||||
|
*
|
||||||
|
* 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 modules/ins/ins_xsens700.h
|
||||||
|
* Xsens700 as a full INS solution
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef INS_XSENS700_H
|
||||||
|
#define INS_XSENS700_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
|
||||||
|
#include "xsens700.h"
|
||||||
|
|
||||||
|
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
|
||||||
|
extern volatile uint8_t new_ins_attitude;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
extern float ins_pitch_neutral;
|
||||||
|
extern float ins_roll_neutral;
|
||||||
|
|
||||||
|
#define DefaultInsImpl ins_xsens700
|
||||||
|
|
||||||
|
extern void ins_xsens700_init(void);
|
||||||
|
extern void ins_xsens700_register(void);
|
||||||
|
extern void ins_xsens700_event(void);
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
#ifndef PRIMARY_GPS
|
||||||
|
#define PRIMARY_GPS gps_xsens700
|
||||||
|
#endif
|
||||||
|
extern void gps_xsens700_init(void);
|
||||||
|
extern void gps_xsens700_register(void);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,373 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||||
|
*
|
||||||
|
* 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 modules/ins/xsens.c
|
||||||
|
* Parser for the Xsens protocol.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "xsens.h"
|
||||||
|
#include "xsens_common.h"
|
||||||
|
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
#include "math/pprz_geodetic_wgs84.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// FIXME Debugging Only
|
||||||
|
#include "mcu_periph/uart.h"
|
||||||
|
#include "pprzlink/messages.h"
|
||||||
|
#include "subsystems/datalink/downlink.h"
|
||||||
|
|
||||||
|
|
||||||
|
/* output mode : calibrated, orientation, position, velocity, status
|
||||||
|
* -----------
|
||||||
|
*
|
||||||
|
* bit 0 temp
|
||||||
|
* bit 1 calibrated
|
||||||
|
* bit 2 orentation
|
||||||
|
* bit 3 aux
|
||||||
|
*
|
||||||
|
* bit 4 position
|
||||||
|
* bit 5 velocity
|
||||||
|
* bit 6-7 Reserved
|
||||||
|
*
|
||||||
|
* bit 8-10 Reserved
|
||||||
|
* bit 11 status
|
||||||
|
*
|
||||||
|
* bit 12 GPS PVT+baro
|
||||||
|
* bit 13 Reserved
|
||||||
|
* bit 14 Raw
|
||||||
|
* bit 15 Reserved
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef XSENS_OUTPUT_MODE
|
||||||
|
#define XSENS_OUTPUT_MODE 0x1836
|
||||||
|
#endif
|
||||||
|
/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED
|
||||||
|
* -----------
|
||||||
|
*
|
||||||
|
* bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc
|
||||||
|
* bit 23 0=quaternion, 1=euler, 2=DCM
|
||||||
|
*
|
||||||
|
* bit 4 1=disable acc output
|
||||||
|
* bit 5 1=disable gyro output
|
||||||
|
* bit 6 1=disable magneto output
|
||||||
|
* bit 7 Reserved
|
||||||
|
*
|
||||||
|
* bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32
|
||||||
|
* bit 10 1=disable aux analog 1
|
||||||
|
* bit 11 1=disable aux analog 2
|
||||||
|
*
|
||||||
|
* bit 12-15 0-only: 14-16 WGS84
|
||||||
|
*
|
||||||
|
* bit 16-19 0-only: 16-18 m/s XYZ
|
||||||
|
*
|
||||||
|
* bit 20-23 Reserved
|
||||||
|
*
|
||||||
|
* bit 24-27 Reseverd
|
||||||
|
*
|
||||||
|
* bit 28-30 Reseverd
|
||||||
|
* bit 31 0=X-North-Z-Up, 1=North-East-Down
|
||||||
|
*/
|
||||||
|
#ifndef XSENS_OUTPUT_SETTINGS
|
||||||
|
#define XSENS_OUTPUT_SETTINGS 0x80000C05
|
||||||
|
#endif
|
||||||
|
|
||||||
|
uint8_t xsens_errorcode;
|
||||||
|
uint8_t xsens_msg_status;
|
||||||
|
uint16_t xsens_time_stamp;
|
||||||
|
uint16_t xsens_output_mode;
|
||||||
|
uint32_t xsens_output_settings;
|
||||||
|
|
||||||
|
|
||||||
|
float xsens_declination = 0;
|
||||||
|
float xsens_gps_arm_x = 0;
|
||||||
|
float xsens_gps_arm_y = 0;
|
||||||
|
float xsens_gps_arm_z = 0;
|
||||||
|
|
||||||
|
volatile int xsens_configured = 0;
|
||||||
|
|
||||||
|
struct Xsens xsens;
|
||||||
|
|
||||||
|
void parse_xsens_buffer(uint8_t c);
|
||||||
|
|
||||||
|
void xsens_init(void)
|
||||||
|
{
|
||||||
|
xsens_status = UNINIT;
|
||||||
|
xsens_configured = 20;
|
||||||
|
|
||||||
|
xsens_msg_status = 0;
|
||||||
|
xsens_time_stamp = 0;
|
||||||
|
xsens_output_mode = XSENS_OUTPUT_MODE;
|
||||||
|
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
|
||||||
|
}
|
||||||
|
|
||||||
|
void xsens_periodic(void)
|
||||||
|
{
|
||||||
|
if (xsens_configured > 0) {
|
||||||
|
switch (xsens_configured) {
|
||||||
|
case 20:
|
||||||
|
/* send mode and settings to MT */
|
||||||
|
XSENS_GoToConfig();
|
||||||
|
XSENS_SetOutputMode(xsens_output_mode);
|
||||||
|
XSENS_SetOutputSettings(xsens_output_settings);
|
||||||
|
break;
|
||||||
|
case 18:
|
||||||
|
// Give pulses on SyncOut
|
||||||
|
XSENS_SetSyncOutSettings(0, 0x0002);
|
||||||
|
break;
|
||||||
|
case 17:
|
||||||
|
// 1 pulse every 100 samples
|
||||||
|
XSENS_SetSyncOutSettings(1, 100);
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
XSENS_ReqLeverArmGps();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 6:
|
||||||
|
XSENS_ReqMagneticDeclination();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 13:
|
||||||
|
#ifdef AHRS_H_X
|
||||||
|
#pragma message "Sending XSens Magnetic Declination."
|
||||||
|
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
|
||||||
|
XSENS_SetMagneticDeclination(xsens_declination);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
case 12:
|
||||||
|
#ifdef GPS_IMU_LEVER_ARM_X
|
||||||
|
#pragma message "Sending XSens GPS Arm."
|
||||||
|
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X, GPS_IMU_LEVER_ARM_Y, GPS_IMU_LEVER_ARM_Z);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
case 10: {
|
||||||
|
uint8_t baud = 1;
|
||||||
|
XSENS_SetBaudrate(baud);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
XSENS_GoToMeasurment();
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
xsens_configured--;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
RunOnceEvery(100, XSENS_ReqGPSStatus());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void parse_xsens_msg(void)
|
||||||
|
{
|
||||||
|
uint8_t offset = 0;
|
||||||
|
if (xsens_id == XSENS_ReqOutputModeAck_ID) {
|
||||||
|
xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
|
||||||
|
} else if (xsens_id == XSENS_ReqOutputSettings_ID) {
|
||||||
|
xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
|
||||||
|
} else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
|
||||||
|
xsens_declination = DegOfRad(XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf));
|
||||||
|
|
||||||
|
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x,
|
||||||
|
&xsens_gps_arm_y, &xsens_gps_arm_z);
|
||||||
|
} else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
|
||||||
|
xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
|
||||||
|
xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
|
||||||
|
xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
|
||||||
|
|
||||||
|
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x,
|
||||||
|
&xsens_gps_arm_y, &xsens_gps_arm_z);
|
||||||
|
} else if (xsens_id == XSENS_Error_ID) {
|
||||||
|
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
|
||||||
|
}
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
else if (xsens_id == XSENS_GPSStatus_ID) {
|
||||||
|
xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
|
||||||
|
xsens.gps.num_sv = 0;
|
||||||
|
|
||||||
|
uint8_t i;
|
||||||
|
// Do not write outside buffer
|
||||||
|
for (i = 0; i < Min(xsens.gps.nb_channels, GPS_NB_CHANNELS); i++) {
|
||||||
|
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf, i);
|
||||||
|
if (ch > xsens.gps.nb_channels) { continue; }
|
||||||
|
xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
|
||||||
|
xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
|
||||||
|
xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
|
||||||
|
xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
|
||||||
|
if (xsens.gps.svinfos[ch].flags > 0) {
|
||||||
|
xsens.gps.num_sv++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
else if (xsens_id == XSENS_MTData_ID) {
|
||||||
|
/* test RAW modes else calibrated modes */
|
||||||
|
//if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
|
||||||
|
if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
|
||||||
|
/* should we write raw data to separate struct? */
|
||||||
|
xsens.gyro.p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf, offset);
|
||||||
|
xsens.gyro.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf, offset);
|
||||||
|
xsens.gyro.r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf, offset);
|
||||||
|
xsens.gyro_available = TRUE;
|
||||||
|
offset += XSENS_DATA_RAWInertial_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
|
||||||
|
#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
|
||||||
|
xsens.gps.week = 0; // FIXME
|
||||||
|
xsens.gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf, offset) * 10;
|
||||||
|
xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf, offset);
|
||||||
|
xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf, offset);
|
||||||
|
xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset);
|
||||||
|
SetBit(xsens.gps.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||||
|
|
||||||
|
// Compute geoid (MSL) height
|
||||||
|
float hmsl = wgs84_ellipsoid_to_geoid_i(xsens.gps.lla_pos.lat, xsens.gps.lla_pos.lon);
|
||||||
|
xsens.gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - (hmsl * 1000.0f);
|
||||||
|
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
|
||||||
|
|
||||||
|
xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset);
|
||||||
|
xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset);
|
||||||
|
xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset);
|
||||||
|
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||||
|
xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100;
|
||||||
|
xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100;
|
||||||
|
xsens.gps.pdop = 5; // FIXME Not output by XSens
|
||||||
|
|
||||||
|
xsens.gps_available = TRUE;
|
||||||
|
#endif
|
||||||
|
offset += XSENS_DATA_RAWGPS_LENGTH;
|
||||||
|
}
|
||||||
|
//} else {
|
||||||
|
if (XSENS_MASK_Temp(xsens_output_mode)) {
|
||||||
|
offset += XSENS_DATA_Temp_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_Calibrated(xsens_output_mode)) {
|
||||||
|
uint8_t l = 0;
|
||||||
|
if (!XSENS_MASK_AccOut(xsens_output_settings)) {
|
||||||
|
xsens.accel.x = XSENS_DATA_Calibrated_accX(xsens_msg_buf, offset);
|
||||||
|
xsens.accel.y = XSENS_DATA_Calibrated_accY(xsens_msg_buf, offset);
|
||||||
|
xsens.accel.z = XSENS_DATA_Calibrated_accZ(xsens_msg_buf, offset);
|
||||||
|
xsens.accel_available = TRUE;
|
||||||
|
l++;
|
||||||
|
}
|
||||||
|
if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
|
||||||
|
xsens.gyro.p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf, offset);
|
||||||
|
xsens.gyro.q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf, offset);
|
||||||
|
xsens.gyro.r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf, offset);
|
||||||
|
xsens.gyro_available = TRUE;
|
||||||
|
l++;
|
||||||
|
}
|
||||||
|
if (!XSENS_MASK_MagOut(xsens_output_settings)) {
|
||||||
|
xsens.mag.x = XSENS_DATA_Calibrated_magX(xsens_msg_buf, offset);
|
||||||
|
xsens.mag.y = XSENS_DATA_Calibrated_magY(xsens_msg_buf, offset);
|
||||||
|
xsens.mag.z = XSENS_DATA_Calibrated_magZ(xsens_msg_buf, offset);
|
||||||
|
xsens.mag_available = TRUE;
|
||||||
|
l++;
|
||||||
|
}
|
||||||
|
offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_Orientation(xsens_output_mode)) {
|
||||||
|
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
|
||||||
|
xsens.quat.qi = XSENS_DATA_Quaternion_q0(xsens_msg_buf, offset);
|
||||||
|
xsens.quat.qx = XSENS_DATA_Quaternion_q1(xsens_msg_buf, offset);
|
||||||
|
xsens.quat.qy = XSENS_DATA_Quaternion_q2(xsens_msg_buf, offset);
|
||||||
|
xsens.quat.qz = XSENS_DATA_Quaternion_q3(xsens_msg_buf, offset);
|
||||||
|
//float_eulers_of_quat(&xsens.euler, &xsens.quat);
|
||||||
|
offset += XSENS_DATA_Quaternion_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
|
||||||
|
xsens.euler.phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
xsens.euler.theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
xsens.euler.psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
offset += XSENS_DATA_Euler_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
|
||||||
|
offset += XSENS_DATA_Matrix_LENGTH;
|
||||||
|
}
|
||||||
|
xsens.new_attitude = TRUE;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
|
||||||
|
uint8_t l = 0;
|
||||||
|
if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
|
||||||
|
l++;
|
||||||
|
}
|
||||||
|
if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
|
||||||
|
l++;
|
||||||
|
}
|
||||||
|
offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_Position(xsens_output_mode)) {
|
||||||
|
xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf, offset));
|
||||||
|
xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset));
|
||||||
|
xsens.lla_f.alt = XSENS_DATA_Position_alt(xsens_msg_buf, offset);
|
||||||
|
offset += XSENS_DATA_Position_LENGTH;
|
||||||
|
|
||||||
|
#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
|
||||||
|
LLA_BFP_OF_REAL(xsens.gps.lla_pos, xsens.lla_f);
|
||||||
|
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||||
|
xsens.gps_available = TRUE;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_Velocity(xsens_output_mode)) {
|
||||||
|
xsens.vel.x = XSENS_DATA_Velocity_vx(xsens_msg_buf, offset);
|
||||||
|
xsens.vel.y = XSENS_DATA_Velocity_vy(xsens_msg_buf, offset);
|
||||||
|
xsens.vel.z = XSENS_DATA_Velocity_vz(xsens_msg_buf, offset);
|
||||||
|
offset += XSENS_DATA_Velocity_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_Status(xsens_output_mode)) {
|
||||||
|
xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf, offset);
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
if (bit_is_set(xsens_msg_status, 2)) { xsens.gps.fix = GPS_FIX_3D; } // gps fix
|
||||||
|
else if (bit_is_set(xsens_msg_status, 1)) { xsens.gps.fix = 0x01; } // efk valid
|
||||||
|
else { xsens.gps.fix = GPS_FIX_NONE; }
|
||||||
|
#ifdef GPS_LED
|
||||||
|
if (xsens.gps.fix == GPS_FIX_3D) {
|
||||||
|
LED_ON(GPS_LED);
|
||||||
|
} else {
|
||||||
|
LED_TOGGLE(GPS_LED);
|
||||||
|
}
|
||||||
|
#endif // GPS_LED
|
||||||
|
#endif // USE_GPS_XSENS
|
||||||
|
offset += XSENS_DATA_Status_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
|
||||||
|
xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf, offset);
|
||||||
|
offset += XSENS_DATA_TimeStamp_LENGTH;
|
||||||
|
}
|
||||||
|
if (XSENS_MASK_UTC(xsens_output_settings)) {
|
||||||
|
xsens.time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf, offset);
|
||||||
|
xsens.time.min = XSENS_DATA_UTC_min(xsens_msg_buf, offset);
|
||||||
|
xsens.time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf, offset);
|
||||||
|
xsens.time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf, offset);
|
||||||
|
xsens.time.year = XSENS_DATA_UTC_year(xsens_msg_buf, offset);
|
||||||
|
xsens.time.month = XSENS_DATA_UTC_month(xsens_msg_buf, offset);
|
||||||
|
xsens.time.day = XSENS_DATA_UTC_day(xsens_msg_buf, offset);
|
||||||
|
|
||||||
|
offset += XSENS_DATA_UTC_LENGTH;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010 ENAC
|
||||||
|
*
|
||||||
|
* 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 modules/ins/xsens.h
|
||||||
|
* Parser for the Xsens protocol.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef XSENS_H
|
||||||
|
#define XSENS_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "math/pprz_algebra_float.h"
|
||||||
|
#include "math/pprz_geodetic_float.h"
|
||||||
|
#include "math/pprz_geodetic_int.h"
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
#include "subsystems/gps.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct XsensTime {
|
||||||
|
int8_t hour;
|
||||||
|
int8_t min;
|
||||||
|
int8_t sec;
|
||||||
|
int32_t nanosec;
|
||||||
|
int16_t year;
|
||||||
|
int8_t month;
|
||||||
|
int8_t day;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Xsens {
|
||||||
|
struct XsensTime time;
|
||||||
|
uint16_t time_stamp;
|
||||||
|
|
||||||
|
struct FloatRates gyro;
|
||||||
|
struct FloatVect3 accel;
|
||||||
|
struct FloatVect3 mag;
|
||||||
|
|
||||||
|
struct LlaCoor_f lla_f;
|
||||||
|
struct FloatVect3 vel; ///< NED velocity in m/s
|
||||||
|
|
||||||
|
struct FloatQuat quat;
|
||||||
|
struct FloatEulers euler;
|
||||||
|
|
||||||
|
volatile bool_t msg_received;
|
||||||
|
volatile bool_t new_attitude;
|
||||||
|
|
||||||
|
bool_t gyro_available;
|
||||||
|
bool_t accel_available;
|
||||||
|
bool_t mag_available;
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
struct GpsState gps;
|
||||||
|
bool_t gps_available;
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
extern struct Xsens xsens;
|
||||||
|
|
||||||
|
extern void xsens_init(void);
|
||||||
|
extern void xsens_periodic(void);
|
||||||
|
extern void parse_xsens_msg(void);
|
||||||
|
|
||||||
|
#endif /* XSENS_H */
|
||||||
@@ -0,0 +1,285 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013 Christophe De Wagter
|
||||||
|
*
|
||||||
|
* 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 modules/ins/xsens700.c
|
||||||
|
* Parser for the Xsens700 protocol.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "xsens700.h"
|
||||||
|
#include "xsens_common.h"
|
||||||
|
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
#include "math/pprz_geodetic_wgs84.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// FIXME Debugging Only
|
||||||
|
#include "mcu_periph/uart.h"
|
||||||
|
#include "pprzlink/messages.h"
|
||||||
|
#include "subsystems/datalink/downlink.h"
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t xsens_errorcode;
|
||||||
|
uint32_t xsens_msg_statusword;
|
||||||
|
uint16_t xsens_time_stamp;
|
||||||
|
uint16_t xsens_output_mode;
|
||||||
|
uint32_t xsens_output_settings;
|
||||||
|
|
||||||
|
float xsens_gps_arm_x = 0;
|
||||||
|
float xsens_gps_arm_y = 0;
|
||||||
|
float xsens_gps_arm_z = 0;
|
||||||
|
|
||||||
|
struct Xsens xsens700;
|
||||||
|
|
||||||
|
volatile int xsens_configured = 0;
|
||||||
|
|
||||||
|
void xsens700_init(void)
|
||||||
|
{
|
||||||
|
xsens_status = UNINIT;
|
||||||
|
xsens_configured = 30;
|
||||||
|
|
||||||
|
xsens_msg_statusword = 0;
|
||||||
|
xsens_time_stamp = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void xsens_ask_message_rate(uint8_t c1, uint8_t c2, uint8_t freq)
|
||||||
|
{
|
||||||
|
uint8_t foo = 0;
|
||||||
|
XsensSend1ByAddr(&c1);
|
||||||
|
XsensSend1ByAddr(&c2);
|
||||||
|
XsensSend1ByAddr(&foo);
|
||||||
|
XsensSend1ByAddr(&freq);
|
||||||
|
}
|
||||||
|
|
||||||
|
void xsens700_periodic(void)
|
||||||
|
{
|
||||||
|
if (xsens_configured > 0) {
|
||||||
|
switch (xsens_configured) {
|
||||||
|
case 25:
|
||||||
|
/* send mode and settings to MT */
|
||||||
|
XSENS_GoToConfig();
|
||||||
|
//XSENS_SetOutputMode(xsens_output_mode);
|
||||||
|
//XSENS_SetOutputSettings(xsens_output_settings);
|
||||||
|
break;
|
||||||
|
case 18:
|
||||||
|
// Give pulses on SyncOut
|
||||||
|
//XSENS_SetSyncOutSettings(0,0x0002);
|
||||||
|
break;
|
||||||
|
case 17:
|
||||||
|
|
||||||
|
XsensHeader(XSENS_SetOutputConfiguration_ID, 44);
|
||||||
|
xsens_ask_message_rate(0x10, 0x10, 4); // UTC
|
||||||
|
xsens_ask_message_rate(0x20, 0x30, 100); // Attitude Euler
|
||||||
|
xsens_ask_message_rate(0x40, 0x10, 100); // Delta-V
|
||||||
|
xsens_ask_message_rate(0x80, 0x20, 100); // Rate of turn
|
||||||
|
xsens_ask_message_rate(0xE0, 0x20, 50); // Status
|
||||||
|
xsens_ask_message_rate(0x30, 0x10, 50); // Baro Pressure
|
||||||
|
xsens_ask_message_rate(0x88, 0x40, 1); // NavSol
|
||||||
|
xsens_ask_message_rate(0x88, 0xA0, 1); // SV Info
|
||||||
|
xsens_ask_message_rate(0x50, 0x20, 50); // GPS Altitude Ellipsiod
|
||||||
|
xsens_ask_message_rate(0x50, 0x40, 50); // GPS Position
|
||||||
|
xsens_ask_message_rate(0xD0, 0x10, 50); // GPS Speed
|
||||||
|
XsensTrailer();
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
//XSENS_ReqLeverArmGps();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 6:
|
||||||
|
//XSENS_ReqMagneticDeclination();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 13:
|
||||||
|
//#ifdef AHRS_H_X
|
||||||
|
//#pragma message "Sending XSens Magnetic Declination."
|
||||||
|
//xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
|
||||||
|
//XSENS_SetMagneticDeclination(xsens_declination);
|
||||||
|
//#endif
|
||||||
|
break;
|
||||||
|
case 12:
|
||||||
|
#ifdef GPS_IMU_LEVER_ARM_X
|
||||||
|
#pragma message "Sending XSens GPS Arm."
|
||||||
|
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X, GPS_IMU_LEVER_ARM_Y, GPS_IMU_LEVER_ARM_Z);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
case 10: {
|
||||||
|
uint8_t baud = 1;
|
||||||
|
XSENS_SetBaudrate(baud);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
XSENS_GoToMeasurment();
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
xsens_configured--;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void parse_xsens700_msg(void)
|
||||||
|
{
|
||||||
|
uint8_t offset = 0;
|
||||||
|
if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
|
||||||
|
xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
|
||||||
|
xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
|
||||||
|
xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
|
||||||
|
} else if (xsens_id == XSENS_Error_ID) {
|
||||||
|
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
|
||||||
|
} else if (xsens_id == XSENS_MTData2_ID) {
|
||||||
|
for (offset = 0; offset < xsens_len;) {
|
||||||
|
uint8_t code1 = xsens_msg_buf[offset];
|
||||||
|
uint8_t code2 = xsens_msg_buf[offset + 1];
|
||||||
|
int subpacklen = (int)xsens_msg_buf[offset + 2];
|
||||||
|
offset += 3;
|
||||||
|
|
||||||
|
if (code1 == 0x10) {
|
||||||
|
if (code2 == 0x10) {
|
||||||
|
// UTC
|
||||||
|
} else if (code2 == 0x20) {
|
||||||
|
// Packet Counter
|
||||||
|
}
|
||||||
|
if (code2 == 0x30) {
|
||||||
|
// ITOW
|
||||||
|
}
|
||||||
|
} else if (code1 == 0x20) {
|
||||||
|
if (code2 == 0x30) {
|
||||||
|
// Attitude Euler
|
||||||
|
xsens700.euler.phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
xsens700.euler.theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
xsens700.euler.psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
|
||||||
|
xsens700.new_attitude = TRUE;
|
||||||
|
}
|
||||||
|
} else if (code1 == 0x40) {
|
||||||
|
if (code2 == 0x10) {
|
||||||
|
// Delta-V
|
||||||
|
xsens700.accel.x = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f;
|
||||||
|
xsens700.accel.y = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f;
|
||||||
|
xsens700.accel.z = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f;
|
||||||
|
}
|
||||||
|
} else if (code1 == 0x80) {
|
||||||
|
if (code2 == 0x20) {
|
||||||
|
// Rate Of Turn
|
||||||
|
xsens700.gyro.p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
xsens700.gyro.q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
xsens700.gyro.r = XSENS_DATA_RateOfTurn_z(xsens_msg_buf, offset) * M_PI / 180;
|
||||||
|
}
|
||||||
|
} else if (code1 == 0x30) {
|
||||||
|
if (code2 == 0x10) {
|
||||||
|
// Baro Pressure
|
||||||
|
}
|
||||||
|
} else if (code1 == 0xE0) {
|
||||||
|
if (code2 == 0x20) {
|
||||||
|
// Status Word
|
||||||
|
xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens_msg_buf, offset);
|
||||||
|
//xsens700.gps.tow = xsens_msg_statusword;
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
|
||||||
|
if (bit_is_set(xsens_msg_statusword, 23) && bit_is_set(xsens_msg_statusword, 24)) {
|
||||||
|
xsens700.gps.fix = GPS_FIX_3D;
|
||||||
|
} else {
|
||||||
|
xsens700.gps.fix = GPS_FIX_2D;
|
||||||
|
}
|
||||||
|
} else { xsens700.gps.fix = GPS_FIX_NONE; }
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
} else if (code1 == 0x88) {
|
||||||
|
if (code2 == 0x40) {
|
||||||
|
xsens700.gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset);
|
||||||
|
xsens700.gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset);
|
||||||
|
xsens700.gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset);
|
||||||
|
xsens700.gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset);
|
||||||
|
xsens700.gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset);
|
||||||
|
} else if (code2 == 0xA0) {
|
||||||
|
// SVINFO
|
||||||
|
xsens700.gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset);
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
xsens700.gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset);
|
||||||
|
|
||||||
|
xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
|
||||||
|
xsens700.gps.last_3dfix_time = sys_time.nb_sec;
|
||||||
|
|
||||||
|
uint8_t i;
|
||||||
|
// Do not write outside buffer
|
||||||
|
for (i = 0; i < Min(xsens700.gps.nb_channels, GPS_NB_CHANNELS); i++) {
|
||||||
|
uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i);
|
||||||
|
if (ch > xsens700.gps.nb_channels) { continue; }
|
||||||
|
xsens700.gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i);
|
||||||
|
xsens700.gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i);
|
||||||
|
xsens700.gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i);
|
||||||
|
xsens700.gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
} else if (code1 == 0x50) {
|
||||||
|
if (code2 == 0x10) {
|
||||||
|
//xsens700.gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f;
|
||||||
|
} else if (code2 == 0x20) {
|
||||||
|
// Altitude Elipsoid
|
||||||
|
xsens700.gps.lla_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f;
|
||||||
|
|
||||||
|
// Compute geoid (MSL) height
|
||||||
|
float geoid_h = wgs84_ellipsoid_to_geoid(xsens700.lla_f.lat, xsens700.lla_f.lon);
|
||||||
|
xsens700.gps.hmsl = xsens700.gps.lla_pos.alt - (geoid_h * 1000.0f);
|
||||||
|
SetBit(xsens700.gps.valid_fields, GPS_VALID_HMSL_BIT);
|
||||||
|
|
||||||
|
//xsens700.gps.tow = geoid_h * 1000.0f; //xsens700.gps.utm_pos.alt;
|
||||||
|
} else if (code2 == 0x40) {
|
||||||
|
// LatLong
|
||||||
|
#ifdef GPS_LED
|
||||||
|
LED_TOGGLE(GPS_LED);
|
||||||
|
#endif
|
||||||
|
xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
|
||||||
|
xsens700.gps.last_3dfix_time = sys_time.nb_sec;
|
||||||
|
xsens700.gps.week = 0; // FIXME
|
||||||
|
|
||||||
|
xsens700.lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset));
|
||||||
|
xsens700.lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset));
|
||||||
|
}
|
||||||
|
} else if (code1 == 0xD0) {
|
||||||
|
if (code2 == 0x10) {
|
||||||
|
// Velocity
|
||||||
|
xsens700.vel.x = XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset);
|
||||||
|
xsens700.vel.y = XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset);
|
||||||
|
xsens700.vel.z = XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset);
|
||||||
|
xsens700.gps.ned_vel.x = xsens700.vel.x;
|
||||||
|
xsens700.gps.ned_vel.y = xsens700.vel.y;
|
||||||
|
xsens700.gps.ned_vel.z = xsens700.vel.x;
|
||||||
|
SetBit(xsens700.gps.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (subpacklen < 0) {
|
||||||
|
subpacklen = 0;
|
||||||
|
}
|
||||||
|
offset += subpacklen;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010 ENAC
|
||||||
|
*
|
||||||
|
* 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 modules/ins/xsens_700.h
|
||||||
|
* Parser for the Xsens protocol.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef XSENS700_H
|
||||||
|
#define XSENS700_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "math/pprz_algebra_float.h"
|
||||||
|
#include "math/pprz_geodetic_float.h"
|
||||||
|
#include "math/pprz_geodetic_int.h"
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
#include "subsystems/gps.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct XsensTime {
|
||||||
|
int8_t hour;
|
||||||
|
int8_t min;
|
||||||
|
int8_t sec;
|
||||||
|
int32_t nanosec;
|
||||||
|
int16_t year;
|
||||||
|
int8_t month;
|
||||||
|
int8_t day;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Xsens {
|
||||||
|
struct XsensTime time;
|
||||||
|
uint16_t time_stamp;
|
||||||
|
|
||||||
|
struct FloatRates gyro;
|
||||||
|
struct FloatVect3 accel;
|
||||||
|
struct FloatVect3 mag;
|
||||||
|
|
||||||
|
struct LlaCoor_f lla_f;
|
||||||
|
struct FloatVect3 vel; ///< NED velocity in m/s
|
||||||
|
|
||||||
|
struct FloatQuat quat;
|
||||||
|
struct FloatEulers euler;
|
||||||
|
|
||||||
|
volatile bool_t msg_received;
|
||||||
|
volatile bool_t new_attitude;
|
||||||
|
|
||||||
|
bool_t gyro_available;
|
||||||
|
bool_t accel_available;
|
||||||
|
bool_t mag_available;
|
||||||
|
|
||||||
|
#if USE_GPS_XSENS
|
||||||
|
struct GpsState gps;
|
||||||
|
bool_t gps_available;
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
extern struct Xsens xsens700;
|
||||||
|
|
||||||
|
extern void xsens700_init(void);
|
||||||
|
extern void xsens700_periodic(void);
|
||||||
|
extern void parse_xsens700_msg(void);
|
||||||
|
|
||||||
|
#endif /* XSENS700_H */
|
||||||
@@ -0,0 +1,105 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||||
|
*
|
||||||
|
* 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 modules/ins/xsens_common.c
|
||||||
|
* Parser for the Xsens protocol.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "xsens_common.h"
|
||||||
|
|
||||||
|
#include "pprzlink/pprzlink_device.h"
|
||||||
|
#include "mcu_periph/uart.h"
|
||||||
|
|
||||||
|
volatile uint8_t xsens_msg_received;
|
||||||
|
|
||||||
|
uint8_t xsens_id;
|
||||||
|
uint8_t xsens_status;
|
||||||
|
uint8_t xsens_len;
|
||||||
|
uint8_t xsens_msg_idx;
|
||||||
|
uint8_t ck;
|
||||||
|
uint8_t send_ck;
|
||||||
|
|
||||||
|
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
|
||||||
|
|
||||||
|
void parse_xsens_buffer(uint8_t c);
|
||||||
|
|
||||||
|
void xsens_event(void)
|
||||||
|
{
|
||||||
|
struct link_device *dev = &((XSENS_LINK).device);
|
||||||
|
if (dev->char_available(dev->periph)) {
|
||||||
|
while (dev->char_available(dev->periph) && !xsens_msg_received) {
|
||||||
|
parse_xsens_buffer(dev->get_byte(dev->periph));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void parse_xsens_buffer(uint8_t c)
|
||||||
|
{
|
||||||
|
ck += c;
|
||||||
|
switch (xsens_status) {
|
||||||
|
case UNINIT:
|
||||||
|
if (c != XSENS_START) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
xsens_status++;
|
||||||
|
ck = 0;
|
||||||
|
break;
|
||||||
|
case GOT_START:
|
||||||
|
if (c != XSENS_BID) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
xsens_status++;
|
||||||
|
break;
|
||||||
|
case GOT_BID:
|
||||||
|
xsens_id = c;
|
||||||
|
xsens_status++;
|
||||||
|
break;
|
||||||
|
case GOT_MID:
|
||||||
|
xsens_len = c;
|
||||||
|
if (xsens_len > XSENS_MAX_PAYLOAD) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
xsens_msg_idx = 0;
|
||||||
|
xsens_status++;
|
||||||
|
break;
|
||||||
|
case GOT_LEN:
|
||||||
|
xsens_msg_buf[xsens_msg_idx] = c;
|
||||||
|
xsens_msg_idx++;
|
||||||
|
if (xsens_msg_idx >= xsens_len) {
|
||||||
|
xsens_status++;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case GOT_DATA:
|
||||||
|
if (ck != 0) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
xsens_msg_received = TRUE;
|
||||||
|
goto restart;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
error:
|
||||||
|
restart:
|
||||||
|
xsens_status = UNINIT;
|
||||||
|
return;
|
||||||
|
}
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||||
|
*
|
||||||
|
* 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 modules/ins/xsens_common.h
|
||||||
|
* Parser for the Xsens protocol.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef XSENS_COMMON_H
|
||||||
|
#define XSENS_COMMON_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
|
||||||
|
/** Includes macros generated from xsens_MTi-G.xml */
|
||||||
|
#include "xsens_protocol.h"
|
||||||
|
|
||||||
|
extern uint8_t xsens_id;
|
||||||
|
extern uint8_t xsens_status;
|
||||||
|
extern uint8_t xsens_len;
|
||||||
|
extern uint8_t xsens_msg_idx;
|
||||||
|
extern uint8_t ck;
|
||||||
|
extern uint8_t send_ck;
|
||||||
|
|
||||||
|
#define XsensLinkDevice (&((XSENS_LINK).device))
|
||||||
|
|
||||||
|
#define XsensInitCheksum() { send_ck = 0; }
|
||||||
|
#define XsensUpdateChecksum(c) { send_ck += c; }
|
||||||
|
|
||||||
|
#define XsensUartSend1(c) XsensLinkDevice->put_byte(XsensLinkDevice->periph, c)
|
||||||
|
#define XsensSend1(c) { uint8_t i8=c; XsensUartSend1(i8); XsensUpdateChecksum(i8); }
|
||||||
|
#define XsensSend1ByAddr(x) { XsensSend1(*x); }
|
||||||
|
#define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); }
|
||||||
|
#define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }
|
||||||
|
|
||||||
|
#define XsensHeader(msg_id, len) { \
|
||||||
|
XsensUartSend1(XSENS_START); \
|
||||||
|
XsensInitCheksum(); \
|
||||||
|
XsensSend1(XSENS_BID); \
|
||||||
|
XsensSend1(msg_id); \
|
||||||
|
XsensSend1(len); \
|
||||||
|
}
|
||||||
|
#define XsensTrailer() { uint8_t i8=0x100-send_ck; XsensUartSend1(i8); }
|
||||||
|
|
||||||
|
|
||||||
|
#define XSENS_MAX_PAYLOAD 254
|
||||||
|
extern uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
|
||||||
|
|
||||||
|
#define UNINIT 0
|
||||||
|
#define GOT_START 1
|
||||||
|
#define GOT_BID 2
|
||||||
|
#define GOT_MID 3
|
||||||
|
#define GOT_LEN 4
|
||||||
|
#define GOT_DATA 5
|
||||||
|
#define GOT_CHECKSUM 6
|
||||||
|
|
||||||
|
extern void xsens_event(void);
|
||||||
|
|
||||||
|
#endif /* XSENS_COMMON_H */
|
||||||
Reference in New Issue
Block a user