[imu] refactor imu_xsens

2026-05-30 11:37:06 +08:00 · 2016-02-07 18:07:51 +01:00
parent da1ab3a374
commit 5b876208b0
3 changed files with 144 additions and 3 deletions
@@ -54,8 +54,9 @@
 ## IMU

 ap.CFLAGS += -DUSE_IMU
-ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
-ap.srcs   += $(SRC_MODULES)/ins/ins_xsens.c
+ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/imu_xsens.h\"
+ap.srcs   += $(SRC_MODULES)/ins/xsens.c
+ap.srcs   += $(SRC_MODULES)/ins/imu_xsens.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/imu.c

 ifndef XSENS_UART_BAUD
@@ -63,6 +64,6 @@ ifndef XSENS_UART_BAUD
 endif

 ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
-ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR)
+ap.CFLAGS += -DXSENS_LINK=uart$(XSENS_UART_NR)
 ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
 ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
@@ -0,0 +1,95 @@
+/*
+ * 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 "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