[imu] Vectornav as IMU (gyro and accel data only) (#2032)

* [imu] Vectornav as IMU (gyro and accel data only)

* [vectornav] Added simple Vectornav configuration tool.

conf/modules/imu_vectornav.xml

@@ -0,0 +1,33 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="imu_vectornav">
+  <doc>
+    <description>
+      Vectornav VN-200 over uart used as IMU.
+      The default configuration doesn't show MAG data.
+      Vectornav is factory calibrated
+    </description>
+    <configure name="VN_PORT" value="uart3" description="UART to use"/>
+    <configure name="VN_BAUD" value="B921600" description="UART baudrate"/>
+  </doc>
+  <autoload name="imu_common"/>
+  <autoload name="imu_nps"/>
+  <header>
+    <file name="imu_vectornav.h" dir="modules/imu"/>
+  </header>
+  <init fun="imu_vectornav_init()"/>
+  <periodic fun="imu_vectornav_init()" freq="1"/>
+  <event fun="imu_vectornav_event()"/>
+  <makefile target="!sim|nps|fbw">
+    <configure name="VN_PORT" default="uart3" case="upper|lower"/>
+    <configure name="VN_BAUD" default="B921600"/>
+    <define name="USE_$(VN_PORT_UPPER)"/>
+    <define name="VN_PORT" value="$(VN_PORT_LOWER)"/>
+    <define name="$(VN_PORT_UPPER)_BAUD" value="$(VN_BAUD)"/>
+
+    <file name="vn200_serial.c" dir="peripherals"/>
+    <file name="imu_vectornav.c" dir="modules/imu"/>
+
+    <define name="IMU_TYPE_H" value="modules/imu/imu_vectornav.h" type="string"/>
+  </makefile>
+</module>

sw/airborne/modules/imu/imu_vectornav.c

@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) 2016 Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
+ *
+ * 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/imu/imu_vectornav.c
+ *
+ * Vectornav VN-200 IMU subsystems, to be used with other AHRS/INS algorithms.
+ */
+
+#include "modules/imu/imu_vectornav.h"
+
+// Systime
+#include "mcu_periph/sys_time.h"
+
+// Abi
+#include "subsystems/abi.h"
+
+// Generated
+#include "generated/airframe.h"
+
+struct ImuVectornav imu_vn;
+
+/* no scaling */
+void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
+void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+
+static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
+{
+  static uint16_t last_cnt = 0;
+  static uint16_t sec_cnt = 0;
+
+  sec_cnt = imu_vn.vn_packet.counter -  last_cnt;
+  imu_vn.vn_freq = sec_cnt; // update frequency counter
+
+  pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
+                               &imu_vn.vn_data.timestamp,
+                               &imu_vn.vn_packet.chksm_error,
+                               &imu_vn.vn_packet.hdr_error,
+                               &sec_cnt,
+                               &imu_vn.vn_data.mode,
+                               &imu_vn.vn_data.err,
+                               &imu_vn.vn_data.ypr_u.phi,
+                               &imu_vn.vn_data.ypr_u.theta,
+                               &imu_vn.vn_data.ypr_u.psi);
+  // update counter
+  last_cnt = imu_vn.vn_packet.counter;
+
+  // reset mode
+  imu_vn.vn_data.mode = 0;
+}
+#endif
+
+
+/**
+ * Init IMU struct and set up ABI messages
+ */
+void imu_vectornav_init(void)
+{
+  // Initialize variables
+  imu_vn.vn_status = VNNotTracking;
+  imu_vn.vn_freq = 0;
+
+  // Initialize packet
+  imu_vn.vn_packet.status = VNMsgSync;
+  imu_vn.vn_packet.msg_idx = 0;
+  imu_vn.vn_packet.msg_available = false;
+  imu_vn.vn_packet.chksm_error = 0;
+  imu_vn.vn_packet.hdr_error = 0;
+  imu_vn.vn_packet.overrun_error = 0;
+  imu_vn.vn_packet.noise_error = 0;
+  imu_vn.vn_packet.framing_error = 0;
+
+  // initialize data structs
+  memset(&(imu_vn.vn_data), 0, sizeof(struct VNData));
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
+#endif
+}
+
+
+
+/**
+ * Event function to read and parse data from the serial port
+ */
+void imu_vectornav_event(void)
+{
+  // receive data
+  vn200_event(&(imu_vn.vn_packet));
+
+  // read message
+  if (imu_vn.vn_packet.msg_available) {
+    vn200_read_message(&(imu_vn.vn_packet),&(imu_vn.vn_data));
+    imu_vn.vn_packet.msg_available = false;
+    imu_vectornav_propagate();
+  }
+}
+
+
+/**
+ * Periodic function checks for the frequency of packets,
+ * triggers warning in case the IMU stops sending data
+ * and performs initial configuration if needed
+ */
+void imu_vectornav_periodic(void)
+{
+  static uint16_t last_cnt = 0;
+  static uint16_t sec_cnt = 0;
+
+  sec_cnt = imu_vn.vn_packet.counter -  last_cnt;
+  imu_vn.vn_freq = sec_cnt; // update frequency counter
+
+  // we want at least 75% of periodic frequency to be able to control the airfcraft
+  if (imu_vn.vn_freq >= (PERIODIC_FREQUENCY*0.75)) {
+    imu_vn.vn_status = VNNotTracking;
+  }
+  else {
+    imu_vn.vn_status = VNOK;
+  }
+}
+
+
+/**
+ * Send ABI messages
+ */
+void imu_vectornav_propagate(void)
+{
+  uint64_t tmp = imu_vn.vn_data.nanostamp / 1000;
+  uint32_t now_ts = (uint32_t) tmp;
+
+  // Rates and accel
+  RATES_BFP_OF_REAL(imu.gyro, imu_vn.vn_data.gyro);
+  ACCELS_BFP_OF_REAL(imu.accel, imu_vn.vn_data.accel);
+
+  // Send accel and gyro data separate for other AHRS algorithms
+  AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &imu.gyro);
+  AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &imu.accel);
+}
+

sw/airborne/modules/imu/imu_vectornav.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2016 Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
+ *
+ * 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/imu/imu_vectornav.h
+ *
+ * Vectornav VN-200 IMU subsystems, to be used with other AHRS/INS algorithms.
+ */
+
+#ifndef IMU_VECTORNAV_H
+#define IMU_VECTORNAV_H
+
+// Subsystem
+#include "subsystems/imu.h"
+
+// Peripheral
+#include "peripherals/vn200_serial.h"
+
+
+struct ImuVectornav {
+  // Packet data
+  struct VNPacket vn_packet;///< Packet struct
+  struct VNData vn_data; ///< Data struct
+  enum VNStatus vn_status;  ///< VN status
+  float vn_freq;            ///< data frequency
+};
+
+
+extern struct ImuVectornav imu_vn;
+
+void imu_vectornav_init(void);
+void imu_vectornav_event(void);
+void imu_vectornav_periodic(void);
+void imu_vectornav_propagate(void);
+
+/* no scaling (the WEAK attribute has no effect */
+#ifndef IMU_GYRO_P_SENS_NUM
+#define IMU_GYRO_P_SENS_NUM 1
+#endif
+
+#ifndef IMU_GYRO_P_SENS_DEN
+#define IMU_GYRO_P_SENS_DEN 1
+#endif
+
+#ifndef IMU_GYRO_Q_SENS_NUM
+#define IMU_GYRO_Q_SENS_NUM 1
+#endif
+
+#ifndef IMU_GYRO_Q_SENS_DEN
+#define IMU_GYRO_Q_SENS_DEN 1
+#endif
+
+#ifndef IMU_GYRO_R_SENS_NUM
+#define IMU_GYRO_R_SENS_NUM 1
+#endif
+
+#ifndef IMU_GYRO_R_SENS_DEN
+#define IMU_GYRO_R_SENS_DEN 1
+#endif
+
+#endif /* IMU_VECTORNAV_H */

sw/tools/vectornav_configurator/Makefile

@@ -0,0 +1,4 @@
+all:
+	g++ VectorNavSetup_console.cpp -o vn_console_setup
+clean:
+	rm vn_console_setup