mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 21:07:40 +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"/>
|
||||
</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>
|
||||
|
||||
|
||||
@@ -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"/>
|
||||
|
||||
@@ -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"/>)
|
||||
|
||||
@@ -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">
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -29,54 +30,22 @@
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include "ins_module.h"
|
||||
// hack to not use this in sim/nps
|
||||
#ifndef SITL
|
||||
#include "xsens.h"
|
||||
|
||||
struct XsensTime {
|
||||
int8_t hour;
|
||||
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);
|
||||
#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_xsens
|
||||
|
||||
extern void ins_xsens_init(void);
|
||||
extern void ins_xsens_register(void);
|
||||
extern void ins_xsens_event(void);
|
||||
|
||||
#if USE_GPS_XSENS
|
||||
#ifndef PRIMARY_GPS
|
||||
@@ -86,4 +55,11 @@ extern void gps_xsens_init(void);
|
||||
extern void gps_xsens_register(void);
|
||||
#endif
|
||||
|
||||
#else // SITL
|
||||
|
||||
static inline void xsens_periodic(void) {}
|
||||
static inline void ins_xsens_event(void) {}
|
||||
|
||||
#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