diff --git a/conf/airframes/CDW/cdw_classix.xml b/conf/airframes/CDW/cdw_classix.xml index a3fb9fd86a..e107c0fdad 100644 --- a/conf/airframes/CDW/cdw_classix.xml +++ b/conf/airframes/CDW/cdw_classix.xml @@ -216,9 +216,9 @@ - - - + + + diff --git a/conf/airframes/CDW/cdw_twoseastwenty.xml b/conf/airframes/CDW/cdw_twoseastwenty.xml index e6bc58c9d3..333d4e24f1 100644 --- a/conf/airframes/CDW/cdw_twoseastwenty.xml +++ b/conf/airframes/CDW/cdw_twoseastwenty.xml @@ -36,10 +36,10 @@ - - - - + + + + @@ -70,10 +70,10 @@ - - - - + + + + diff --git a/conf/airframes/CDW/cdw_yapa_xsens.xml b/conf/airframes/CDW/cdw_yapa_xsens.xml index e34c994ff9..9253102bb7 100644 --- a/conf/airframes/CDW/cdw_yapa_xsens.xml +++ b/conf/airframes/CDW/cdw_yapa_xsens.xml @@ -25,10 +25,10 @@ - - - - + + + + diff --git a/conf/airframes/examples/microjet_imu_xsens.xml b/conf/airframes/examples/microjet_imu_xsens.xml index 1ab3a1316b..92d91d3b86 100644 --- a/conf/airframes/examples/microjet_imu_xsens.xml +++ b/conf/airframes/examples/microjet_imu_xsens.xml @@ -21,10 +21,10 @@ - - - - + + + + diff --git a/conf/airframes/examples/microjet_lisa_m_xsens.xml b/conf/airframes/examples/microjet_lisa_m_xsens.xml index ee9ddd2269..7278c17917 100644 --- a/conf/airframes/examples/microjet_lisa_m_xsens.xml +++ b/conf/airframes/examples/microjet_lisa_m_xsens.xml @@ -21,10 +21,10 @@ - - - - + + + + diff --git a/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile b/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile index b18ab3bd5f..67d4ac948e 100644 --- a/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile @@ -1,68 +1 @@ -# Hey Emacs, this is a -*- makefile -*- - -# XSens Mti just providing IMU measurements - -# -# -# -# -# -#
-# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -#
- - -######################################### -## 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 by ) diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index c243ad9f96..038ff5b548 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -1,70 +1 @@ -# Hey Emacs, this is a -*- makefile -*- - -# XSens Mti-G - -# -# -# -# - - - -######################################### -## 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 by ) diff --git a/conf/firmwares/subsystems/shared/ins_xsens700.makefile b/conf/firmwares/subsystems/shared/ins_xsens700.makefile index 58be4d9d3f..bf4eb71b37 100644 --- a/conf/firmwares/subsystems/shared/ins_xsens700.makefile +++ b/conf/firmwares/subsystems/shared/ins_xsens700.makefile @@ -1,64 +1 @@ -# Hey Emacs, this is a -*- makefile -*- - -# XSens Mti-G - -# -# -# -# - - -######################################### -## 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 by ) diff --git a/conf/modules/imu_xsens.xml b/conf/modules/imu_xsens.xml new file mode 100644 index 0000000000..8e464f0d13 --- /dev/null +++ b/conf/modules/imu_xsens.xml @@ -0,0 +1,49 @@ + + + + + + XSens IMU + + + + +
+ +
+ + + + + + + + + + + + + + + ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/imu_xsens.h\" + + + + + + + + + sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" + + + + + + + + nps.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_nps.h\" + + + + diff --git a/conf/modules/ins_xsens.xml b/conf/modules/ins_xsens.xml index 6ec0d450ef..4c93bdcfa8 100644 --- a/conf/modules/ins_xsens.xml +++ b/conf/modules/ins_xsens.xml @@ -1,16 +1,77 @@ - + - XSens + + XSens Mti-G INS + + +
- +
- - - + + + + + + + + + + + + + + + + + + + + + + + ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\" + + + + + + + + + + + + + + + + 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\" + + + + + + + + + + + + + + + + + nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\" + nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" + diff --git a/conf/modules/ins_xsens700.xml b/conf/modules/ins_xsens700.xml new file mode 100644 index 0000000000..4247003015 --- /dev/null +++ b/conf/modules/ins_xsens700.xml @@ -0,0 +1,76 @@ + + + + + + XSens Mti-G 700 INS + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens700.h\" + + + + + + + + + + + + + + + + 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\" + + + + + + + + + + + + + + + + + nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\" + nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" + + + + diff --git a/conf/modules/ins_xsens_MTiG_Uart0.xml b/conf/modules/ins_xsens_MTiG_Uart0.xml deleted file mode 100644 index 0438e3776a..0000000000 --- a/conf/modules/ins_xsens_MTiG_Uart0.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - XSens MTiG - - - -
- -
- - - - - - - - - - - - - - - diff --git a/conf/modules/ins_xsens_MTi_Uart0.xml b/conf/modules/ins_xsens_MTi_Uart0.xml deleted file mode 100644 index 8bcb60694c..0000000000 --- a/conf/modules/ins_xsens_MTi_Uart0.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - XSens MTi - - -
- -
- - - - - - - - - - - - - - diff --git a/sw/airborne/modules/ins/imu_xsens.c b/sw/airborne/modules/ins/imu_xsens.c new file mode 100644 index 0000000000..37c186dd8f --- /dev/null +++ b/sw/airborne/modules/ins/imu_xsens.c @@ -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 */ +} diff --git a/sw/airborne/modules/ins/imu_xsens.h b/sw/airborne/modules/ins/imu_xsens.h new file mode 100644 index 0000000000..d347515a52 --- /dev/null +++ b/sw/airborne/modules/ins/imu_xsens.h @@ -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 diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index f24b6d5f0c..33feafd5ad 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -21,244 +21,28 @@ */ /** @file ins_xsens.c - * Parser for the Xsens protocol. + * Xsens as a full INS solution */ -#include "ins_module.h" #include "ins_xsens.h" +#include "xsens_common.h" #include "subsystems/ins.h" -#include - #include "generated/airframe.h" #include "mcu_periph/sys_time.h" -#include "pprzlink/messages.h" +#include "subsystems/abi.h" +#include "state.h" #if USE_GPS_XSENS #if !USE_GPS #error "USE_GPS needs to be 1 to use the Xsens GPS!" #endif #include "subsystems/gps.h" -#include "subsystems/abi.h" -#include "math/pprz_geodetic_wgs84.h" #include "math/pprz_geodetic_float.h" #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */ -bool_t gps_xsens_msg_available; #endif -// positions -INS_FORMAT ins_x; -INS_FORMAT ins_y; -INS_FORMAT ins_z; - -// velocities -INS_FORMAT ins_vx; -INS_FORMAT ins_vy; -INS_FORMAT ins_vz; - -// body angles -INS_FORMAT ins_phi; -INS_FORMAT ins_theta; -INS_FORMAT ins_psi; - -// angle rates -INS_FORMAT ins_p; -INS_FORMAT ins_q; -INS_FORMAT ins_r; - -// accelerations -INS_FORMAT ins_ax; -INS_FORMAT ins_ay; -INS_FORMAT ins_az; - -// magnetic -INS_FORMAT ins_mx; -INS_FORMAT ins_my; -INS_FORMAT ins_mz; - -#if USE_INS_MODULE -float ins_pitch_neutral; -float ins_roll_neutral; -#endif - - -////////////////////////////////////////////////////////////////////////////////////////// -// -// XSens Specific -// - -volatile uint8_t ins_msg_received; - -#define XsensInitCheksum() { send_ck = 0; } -#define XsensUpdateChecksum(c) { send_ck += c; } - -#define XsensSend1(c) { uint8_t i8=c; InsUartSend1(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) { \ - InsUartSend1(XSENS_START); \ - XsensInitCheksum(); \ - XsensSend1(XSENS_BID); \ - XsensSend1(msg_id); \ - XsensSend1(len); \ - } -#define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); } - -/** Includes macros generated from xsens_MTi-G.xml */ -#include "xsens_protocol.h" - - -#define XSENS_MAX_PAYLOAD 254 -uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; - -/* 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 - -#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 - -// FIXME Debugging Only -#include "mcu_periph/uart.h" -#include "pprzlink/messages.h" -#include "subsystems/datalink/downlink.h" - - -uint8_t xsens_errorcode; -uint8_t xsens_msg_status; -uint16_t xsens_time_stamp; -uint16_t xsens_output_mode; -uint32_t xsens_output_settings; - - -float xsens_declination = 0; -float xsens_gps_arm_x = 0; -float xsens_gps_arm_y = 0; -float xsens_gps_arm_z = 0; - -#if USE_GPS_XSENS -struct LlaCoor_f lla_f; -struct UtmCoor_f utm_f; -#endif - -struct XsensTime xsens_time; - -static uint8_t xsens_id; -static uint8_t xsens_status; -static uint8_t xsens_len; -static uint8_t xsens_msg_idx; -static uint8_t ck; -uint8_t send_ck; - -volatile int xsens_configured = 0; - -void xsens_init(void); - -void xsens_init(void) -{ - - xsens_status = UNINIT; - xsens_configured = 20; - -#if USE_INS_MODULE - ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; - ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; -#endif - - xsens_msg_status = 0; - xsens_time_stamp = 0; - xsens_output_mode = XSENS_OUTPUT_MODE; - xsens_output_settings = XSENS_OUTPUT_SETTINGS; -} - -#if USE_IMU -struct ImuXsens imu_xsens; - -void imu_impl_init(void) -{ - xsens_init(); - imu_xsens.gyro_available = FALSE; - imu_xsens.accel_available = FALSE; - imu_xsens.mag_available = FALSE; -} - -void imu_periodic(void) -{ - xsens_periodic(); -} -#endif /* USE_IMU */ - -#if USE_INS_MODULE -void ins_xsens_update_gps(struct GpsState *gps_s); - -void ins_xsens_init(void) -{ - struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; - stateSetLocalUtmOrigin_f(&utm0); - stateSetPositionUtm_f(&utm0); - - xsens_init(); -} - -#include "subsystems/abi.h" /** ABI binding for gps data. * Used for GPS ABI messages. */ @@ -267,12 +51,15 @@ void ins_xsens_init(void) #endif PRINT_CONFIG_VAR(INS_XSENS_GPS_ID) static abi_event gps_ev; + +float ins_pitch_neutral; +float ins_roll_neutral; + +static void handle_ins_msg(void); +static void update_state_interface(void); static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), - struct GpsState *gps_s) -{ - ins_xsens_update_gps(gps_s); -} + struct GpsState *gps_s); void ins_xsens_register(void) { @@ -280,7 +67,31 @@ void ins_xsens_register(void) AbiBindMsgGPS(INS_XSENS_GPS_ID, &gps_ev, gps_cb); } -void ins_xsens_update_gps(struct GpsState *gps_s) +void ins_xsens_init(void) +{ + xsens_init(); + + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + stateSetPositionUtm_f(&utm0); +} + +void ins_xsens_event(void) +{ + xsens_event(); + if (xsens.msg_received) { + parse_xsens_msg(); + handle_ins_msg(); + xsens.msg_received = FALSE; + } +} + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) { struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; @@ -296,485 +107,85 @@ void ins_xsens_update_gps(struct GpsState *gps_s) // set velocity stateSetSpeedNed_f(&ned_vel); } -#endif #if USE_GPS_XSENS void gps_xsens_init(void) { - gps.nb_channels = 0; + xsens.gps.nb_channels = 0; } static void gps_xsens_publish(void) { // publish gps data uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + xsens.gps.last_msg_ticks = sys_time.nb_sec_rem; + xsens.gps.last_msg_time = sys_time.nb_sec; + if (xsens.gps.fix == GPS_FIX_3D) { + xsens.gps.last_3dfix_ticks = sys_time.nb_sec_rem; + xsens.gps.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &xsens.gps); } #endif -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()); -} - -#if USE_INS_MODULE -#include "state.h" - -static inline void update_state_interface(void) +static void update_state_interface(void) { // Send to Estimator (Control) #ifdef XSENS_BACKWARDS struct FloatEulers att = { - -ins_phi + ins_roll_neutral, - -ins_theta + ins_pitch_neutral, - ins_psi + RadOfDeg(180) + -xsens.euler.phi + ins_roll_neutral, + -xsens.euler.theta + ins_pitch_neutral, + xsens.euler.psi + RadOfDeg(180) }; - struct FloatRates rates = { - -ins_p, - -ins_q, - ins_r + struct FloatEulerstRates rates = { + -xsens.gyro.p, + -xsens.gyro.q, + xsens.gyro.r }; #else struct FloatEulers att = { - ins_phi + ins_roll_neutral, - ins_theta + ins_pitch_neutral, - ins_psi - }; - struct FloatRates rates = { - ins_p, - ins_q, - ins_r + xsens.euler.phi + ins_roll_neutral, + xsens.euler.theta + ins_pitch_neutral, + xsens.euler.psi }; + struct FloatRates rates = xsens.gyro; #endif stateSetNedToBodyEulers_f(&att); stateSetBodyRates_f(&rates); } -#endif /* USE_INS_MODULE */ -void handle_ins_msg(void) + +static void handle_ins_msg(void) { -#if USE_INS_MODULE update_state_interface(); -#endif -#if USE_IMU - uint32_t now_ts = get_sys_time_usec(); -#ifdef XSENS_BACKWARDS - if (imu_xsens.gyro_available) { - RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r)); - imu_xsens.gyro_available = FALSE; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro); + if (xsens.new_attitude) { + new_ins_attitude = TRUE; + xsens.new_attitude = FALSE; } - if (imu_xsens.accel_available) { - VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az)); - imu_xsens.accel_available = FALSE; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel); - } - if (imu_xsens.mag_available) { - VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz)); - imu_xsens.mag_available = FALSE; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag); - } -#else - if (imu_xsens.gyro_available) { - RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r)); - imu_xsens.gyro_available = FALSE; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro); - } - if (imu_xsens.accel_available) { - VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az)); - imu_xsens.accel_available = FALSE; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel); - } - if (imu_xsens.mag_available) { - VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz)); - imu_xsens.mag_available = FALSE; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag); - } -#endif /* XSENS_BACKWARDS */ -#endif /* USE_IMU */ #if USE_GPS_XSENS + if (xsens.gps_available) { + // Horizontal speed + float fspeed = FLOAT_VECT2_NORM(xsens.vel); + if (xsens.gps.fix != GPS_FIX_3D) { + fspeed = 0; + } + xsens.gps.gspeed = fspeed * 100.; + xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100; - // Horizontal speed - float fspeed = sqrt(ins_vx * ins_vx + ins_vy * ins_vy); - if (gps.fix != GPS_FIX_3D) { - fspeed = 0; + float fcourse = atan2f(xsens.vel.y, xsens.vel.x); + xsens.gps.course = fcourse * 1e7; + SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT); + + gps_xsens_publish(); + xsens.gps_available = FALSE; } - gps.gspeed = fspeed * 100.; - gps.speed_3d = (uint16_t)(sqrt(ins_vx * ins_vx + ins_vy * ins_vy + ins_vz * ins_vz) * 100); - - float fcourse = atan2f((float)ins_vy, (float)ins_vx); - gps.course = fcourse * 1e7; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); #endif // USE_GPS_XSENS } -void parse_ins_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) { - gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); - gps.num_sv = 0; - - uint8_t i; - // Do not write outside buffer - for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { - uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf, i); - if (ch > gps.nb_channels) { continue; } - gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); - gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); - gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); - gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i); - if (gps.svinfos[ch].flags > 0) { - gps.num_sv++; - } - } - } -#endif - 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)) { - ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf, offset); - ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf, offset); - ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf, offset); -#if USE_IMU - imu_xsens.gyro_available = TRUE; -#endif - offset += XSENS_DATA_RAWInertial_LENGTH; - } - if (XSENS_MASK_RAWGPS(xsens_output_mode)) { -#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS - - gps.week = 0; // FIXME - gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf, offset) * 10; - gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf, offset); - gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf, offset); - gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - - /* Set the real UTM zone */ - gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); - - ins_x = utm_f.east; - ins_y = utm_f.north; - // Altitude: Xsens LLH gives ellipsoid height - ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) / 1000.; - - // Compute geoid (MSL) height - float hmsl = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); - gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - (hmsl * 1000.0f); - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - - ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset)) / 100.; - ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset)) / 100.; - ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset)) / 100.; - gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset); - gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset); - gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset); - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); - gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100; - gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100; - gps.pdop = 5; // FIXME Not output by XSens - - gps_xsens_publish(); -#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)) { - ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf, offset); - ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf, offset); - ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf, offset); -#if USE_IMU - imu_xsens.accel_available = TRUE; -#endif - l++; - } - if (!XSENS_MASK_GyrOut(xsens_output_settings)) { - ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf, offset); - ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf, offset); - ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf, offset); -#if USE_IMU - imu_xsens.gyro_available = TRUE; -#endif - l++; - } - if (!XSENS_MASK_MagOut(xsens_output_settings)) { - ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf, offset); - ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf, offset); - ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf, offset); -#if USE_IMU - imu_xsens.mag_available = TRUE; -#endif - l++; - } - offset += l * XSENS_DATA_Calibrated_LENGTH / 3; - } - if (XSENS_MASK_Orientation(xsens_output_mode)) { - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) { - float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf, offset); - float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf, offset); - float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf, offset); - float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf, offset); - float dcm00 = 1.0 - 2 * (q2 * q2 + q3 * q3); - float dcm01 = 2 * (q1 * q2 + q0 * q3); - float dcm02 = 2 * (q1 * q3 - q0 * q2); - float dcm12 = 2 * (q2 * q3 + q0 * q1); - float dcm22 = 1.0 - 2 * (q1 * q1 + q2 * q2); - ins_phi = atan2(dcm12, dcm22); - ins_theta = -asin(dcm02); - ins_psi = atan2(dcm01, dcm00); - offset += XSENS_DATA_Quaternion_LENGTH; - } - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) { - ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180; - ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180; - ins_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; - } - new_ins_attitude = 1; - } - 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)) { -#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS - lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf, offset)); - lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset)); - gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7); - gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); - ins_x = utm_f.east; - ins_y = utm_f.north; - ins_z = XSENS_DATA_Position_alt(xsens_msg_buf, offset); //TODO is this hms or above ellipsoid? - gps.hmsl = ins_z * 1000; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - // what about gps.lla_pos.alt and gps.utm_pos.alt ? - - gps_xsens_publish(); -#endif - offset += XSENS_DATA_Position_LENGTH; - } - if (XSENS_MASK_Velocity(xsens_output_mode)) { -#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS - ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf, offset); - ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf, offset); - ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf, offset); -#endif - 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)) { gps.fix = GPS_FIX_3D; } // gps fix - else if (bit_is_set(xsens_msg_status, 1)) { gps.fix = 0x01; } // efk valid - else { gps.fix = GPS_FIX_NONE; } -#ifdef GPS_LED - if (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); -#if USE_GPS_XSENS - gps.tow = xsens_time_stamp; -#endif - 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; - } - //} - } - -} - -void parse_ins_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; - } - ins_msg_received = TRUE; - goto restart; - break; - default: - break; - } - return; -error: -restart: - xsens_status = UNINIT; - return; -} - #ifdef USE_GPS_XSENS /* * register callbacks & structs diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 2b7bf22a9d..9e55c00136 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -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 diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index a9a180616b..0f10a185e2 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -22,15 +22,13 @@ /** * @file modules/ins/ins_xsens700.c - * Parser for the Xsens protocol. + * Xsens700 as a full INS solution */ -#include "ins_module.h" -#include "ins_xsens.h" +#include "ins_xsens700.h" +#include "xsens_common.h" #include "subsystems/ins.h" -#include - #include "generated/airframe.h" #include "mcu_periph/sys_time.h" @@ -48,148 +46,6 @@ #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */ #endif -// positions -INS_FORMAT ins_x; -INS_FORMAT ins_y; -INS_FORMAT ins_z; - -// velocities -INS_FORMAT ins_vx; -INS_FORMAT ins_vy; -INS_FORMAT ins_vz; - -// body angles -INS_FORMAT ins_phi; -INS_FORMAT ins_theta; -INS_FORMAT ins_psi; - -// angle rates -INS_FORMAT ins_p; -INS_FORMAT ins_q; -INS_FORMAT ins_r; - -// accelerations -INS_FORMAT ins_ax; -INS_FORMAT ins_ay; -INS_FORMAT ins_az; - -// magnetic -INS_FORMAT ins_mx; -INS_FORMAT ins_my; -INS_FORMAT ins_mz; - -#if USE_INS_MODULE -float ins_pitch_neutral; -float ins_roll_neutral; -#endif - - -////////////////////////////////////////////////////////////////////////////////////////// -// -// XSens Specific -// - -volatile uint8_t ins_msg_received; - -#define XsensInitCheksum() { send_ck = 0; } -#define XsensUpdateChecksum(c) { send_ck += c; } - -#define XsensSend1(c) { uint8_t i8=c; InsUartSend1(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) { \ - InsUartSend1(XSENS_START); \ - XsensInitCheksum(); \ - XsensSend1(XSENS_BID); \ - XsensSend1(msg_id); \ - XsensSend1(len); \ - } -#define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); } - -/** Includes macros generated from xsens_MTi-G.xml */ -#include "xsens_protocol.h" - - -#define XSENS_MAX_PAYLOAD 254 -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 - -// 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_declination = 0; -float xsens_gps_arm_x = 0; -float xsens_gps_arm_y = 0; -float xsens_gps_arm_z = 0; - - -int8_t xsens_hour; -int8_t xsens_min; -int8_t xsens_sec; -int32_t xsens_nanosec; -int16_t xsens_year; -int8_t xsens_month; -int8_t xsens_day; - -static uint8_t xsens_id; -static uint8_t xsens_status; -static uint8_t xsens_len; -static uint8_t xsens_msg_idx; -static uint8_t ck; -uint8_t send_ck; - -struct LlaCoor_f lla_f; -struct UtmCoor_f utm_f; - -volatile int xsens_configured = 0; - -void xsens_init(void); - -void xsens_init(void) -{ - - xsens_status = UNINIT; - xsens_configured = 30; - - xsens_msg_statusword = 0; - xsens_time_stamp = 0; - -} - -#if USE_INS_MODULE -void ins_xsens_update_gps(struct GpsState *gps_s); - -void ins_xsens_init(void) -{ - struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; - stateSetLocalUtmOrigin_f(&utm0); - stateSetPositionUtm_f(&utm0); - - ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; - ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; - - xsens_init(); -} - -#include "subsystems/abi.h" /** ABI binding for gps data. * Used for GPS ABI messages. */ @@ -198,20 +54,47 @@ void ins_xsens_init(void) #endif PRINT_CONFIG_VAR(INS_XSENS700_GPS_ID) static abi_event gps_ev; + +float ins_pitch_neutral; +float ins_roll_neutral; + +static void handle_ins_msg(void); +static void update_state_interface(void); static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), - struct GpsState *gps_s) -{ - ins_xsens_update_gps(gps_s); -} + struct GpsState *gps_s); -void ins_xsens_register(void) +void ins_xsens700_register(void) { - ins_register_impl(ins_xsens_init); + ins_register_impl(ins_xsens700_init); AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); } -void ins_xsens_update_gps(struct GpsState *gps_s) +void ins_xsens700_init(void) +{ + xsens700_init(); + + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + stateSetPositionUtm_f(&utm0); +} + +void ins_xsens700_event(void) +{ + xsens_event(); + if (xsens700.msg_received) { + parse_xsens700_msg(); + handle_ins_msg(); + xsens700.msg_received = FALSE; + } +} + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) { struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; @@ -227,409 +110,97 @@ void ins_xsens_update_gps(struct GpsState *gps_s) // set velocity stateSetSpeedNed_f(&ned_vel); } -#endif + #if USE_GPS_XSENS -void gps_xsens_init(void) +void gps_xsens700_init(void) { - gps.nb_channels = 0; + xsens700.gps.nb_channels = 0; } -static void gps_xsens_publish(void) +static void gps_xsens700_publish(void) { // publish gps data uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + xsens700.gps.last_msg_ticks = sys_time.nb_sec_rem; + xsens700.gps.last_msg_time = sys_time.nb_sec; + if (xsens700.gps.fix == GPS_FIX_3D) { + xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem; + xsens700.gps.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &xsens700.gps); } #endif -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 xsens_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; - } - - //RunOnceEvery(100,XSENS_ReqGPSStatus()); -} - -#if USE_INS_MODULE -#include "state.h" - -static inline void update_state_interface(void) +static void update_state_interface(void) { // Send to Estimator (Control) -#if XSENS_BACKWARDS +#ifdef XSENS_BACKWARDS struct FloatEulers att = { - ins_phi + ins_roll_neutral, - -ins_theta + ins_pitch_neutral, - -ins_psi + RadOfDeg(180) + xsens700.euler.phi + ins_roll_neutral, + -xsens700.euler.theta + ins_pitch_neutral, + -xsens700.euler.psi + RadOfDeg(180) }; - struct FloatRates rates = { - ins_p, - -ins_q, - -ins_r + struct FloatEulerstRates rates = { + xsens700.gyro.p, + -xsens700.gyro.q, + -xsens700.gyro.r }; #else struct FloatEulers att = { - -ins_phi + ins_roll_neutral, - ins_theta + ins_pitch_neutral, - -ins_psi + -xsens700.euler.phi + ins_roll_neutral, + xsens700.euler.theta + ins_pitch_neutral, + -xsens700.euler.psi }; - struct FloatRates rates = { - -ins_p, - ins_q, - -ins_r + struct FloatRates rates = { + -xsens700.gyro.p, + xsens700.gyro.q, + -xsens700.gyro.r }; #endif stateSetNedToBodyEulers_f(&att); stateSetBodyRates_f(&rates); } -#endif /* USE_INS_MODULE */ + void handle_ins_msg(void) { -#if USE_INS_MODULE update_state_interface(); -#endif + + if (xsens700.new_attitude) { + new_ins_attitude = TRUE; + xsens700.new_attitude = FALSE; + } #if USE_GPS_XSENS - // Horizontal speed - float fspeed = sqrt(ins_vx * ins_vx + ins_vy * ins_vy); - if (gps.fix != GPS_FIX_3D) { - fspeed = 0; - } - gps.gspeed = fspeed * 100.; - gps.speed_3d = (uint16_t)(sqrt(ins_vx * ins_vx + ins_vy * ins_vy + ins_vz * ins_vz) * 100); + if (xsens700.gps_available) { + // Horizontal speed + float fspeed = FLOAT_VECT2_NORM(xsens700.vel); + if (xsens700.gps.fix != GPS_FIX_3D) { + fspeed = 0; + } + xsens700.gps.gspeed = fspeed * 100.; + xsens700.gps.speed_3d = float_vect3_norm(&xsens700.vel) * 100; - float fcourse = atan2f((float)ins_vy, (float)ins_vx); - gps.course = fcourse * 1e7; + float fcourse = atan2f(xsens700.vel.y, xsens700.vel.x); + xsens700.gps.course = fcourse * 1e7; + SetBit(xsens700.gps.valid_fields, GPS_VALID_COURSE_BIT); + + gps_xsens700_publish(); + xsens700.gps_available = FALSE; + } #endif // USE_GPS_XSENS } -void parse_ins_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); - - 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); - } 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 - ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180; - ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180; - ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180; - - new_ins_attitude = 1; - - } - } else if (code1 == 0x40) { - if (code2 == 0x10) { - // Delta-V - ins_ax = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f; - ins_ay = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f; - ins_az = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f; - } - } else if (code1 == 0x80) { - if (code2 == 0x20) { - // Rate Of Turn - ins_p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180; - ins_q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180; - ins_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); - //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)) { - gps.fix = GPS_FIX_3D; - } else { - gps.fix = GPS_FIX_2D; - } - } else { gps.fix = GPS_FIX_NONE; } -#endif - } - } else if (code1 == 0x88) { - if (code2 == 0x40) { - gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset); - gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset); - gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset); - gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset); - gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset); - } else if (code2 == 0xA0) { - // SVINFO - gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset); - -#if USE_GPS_XSENS - gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset); - - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; - - uint8_t i; - // Do not write outside buffer - for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { - uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i); - if (ch > gps.nb_channels) { continue; } - gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i); - gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i); - gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i); - gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i); - } -#endif - } - } else if (code1 == 0x50) { - if (code2 == 0x10) { - //gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f; - } else if (code2 == 0x20) { - // Altitude Elipsoid - gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f; - - // Compute geoid (MSL) height - float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); - gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f); - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - - //gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt; - } else if (code2 == 0x40) { - // LatLong -#ifdef GPS_LED - LED_TOGGLE(GPS_LED); -#endif - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; - gps.week = 0; // FIXME - lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset)); - lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset)); - - // Set the real UTM zone - gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1; - - utm_f.zone = nav_utm_zone0; - // convert to utm - utm_of_lla_f(&utm_f, &lla_f); - // copy results of utm conversion - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); - - gps_xsens_publish(); - } - } else if (code1 == 0xD0) { - if (code2 == 0x10) { - // Velocity - ins_vx = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset)); - ins_vy = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset)); - ins_vz = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset)); - gps.ned_vel.x = ins_vx; - gps.ned_vel.y = ins_vy; - gps.ned_vel.z = ins_vx; - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); - } - } - - if (subpacklen < 0) { - subpacklen = 0; - } - offset += subpacklen; - } - - - /* - - gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100; - gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100; - gps.pdop = 5; // FIXME Not output by XSens - */ - - /* - */ - - - /* - ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset); - ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset); - ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset); - */ - - - /* - xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset); - #if USE_GPS_XSENS - gps.tow = xsens_time_stamp; - #endif - */ - - } - -} - - -void parse_ins_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; - } - ins_msg_received = TRUE; - goto restart; - break; - default: - break; - } - return; -error: -restart: - xsens_status = UNINIT; - return; -} #ifdef USE_GPS_XSENS /* * register callbacks & structs */ -void gps_xsens_register(void) +void gps_xsens700_register(void) { - gps_register_impl(gps_xsens_init, NULL, GPS_XSENS_ID); + gps_register_impl(gps_xsens700_init, NULL, GPS_XSENS_ID); } #endif diff --git a/sw/airborne/modules/ins/ins_xsens700.h b/sw/airborne/modules/ins/ins_xsens700.h new file mode 100644 index 0000000000..75bd5fc380 --- /dev/null +++ b/sw/airborne/modules/ins/ins_xsens700.h @@ -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 diff --git a/sw/airborne/modules/ins/xsens.c b/sw/airborne/modules/ins/xsens.c new file mode 100644 index 0000000000..f0ae21cf52 --- /dev/null +++ b/sw/airborne/modules/ins/xsens.c @@ -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; + } + } + +} diff --git a/sw/airborne/modules/ins/xsens.h b/sw/airborne/modules/ins/xsens.h new file mode 100644 index 0000000000..b8a3f07930 --- /dev/null +++ b/sw/airborne/modules/ins/xsens.h @@ -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 */ diff --git a/sw/airborne/modules/ins/xsens700.c b/sw/airborne/modules/ins/xsens700.c new file mode 100644 index 0000000000..c9e76ac923 --- /dev/null +++ b/sw/airborne/modules/ins/xsens700.c @@ -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; + } + } +} diff --git a/sw/airborne/modules/ins/xsens700.h b/sw/airborne/modules/ins/xsens700.h new file mode 100644 index 0000000000..b5d0009fec --- /dev/null +++ b/sw/airborne/modules/ins/xsens700.h @@ -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 */ diff --git a/sw/airborne/modules/ins/xsens_common.c b/sw/airborne/modules/ins/xsens_common.c new file mode 100644 index 0000000000..4c2c9ba95a --- /dev/null +++ b/sw/airborne/modules/ins/xsens_common.c @@ -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; +} diff --git a/sw/airborne/modules/ins/xsens_common.h b/sw/airborne/modules/ins/xsens_common.h new file mode 100644 index 0000000000..8abd2dc9ed --- /dev/null +++ b/sw/airborne/modules/ins/xsens_common.h @@ -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 */