[subsystems][imu][ahrs] Drivers for CH Robotic UM6 IMU/AHRS (see www.…

…chrobotics.com).

Generic AHRS for all external AHRS systems (such as GX3 etc) added.
closes paparazzi#347

conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile

@@ -0,0 +1,17 @@
+#
+# AHRS wrapper for AHRS devices, such as GX3 or UM6
+# 2013, Utah State University, http://aggieair.usu.edu/
+
+AHRS_CFLAGS  = -DUSE_AHRS
+
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_extern_quat.h\"
+AHRS_SRCS   += subsystems/ahrs.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_extern_quat.c
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)

conf/firmwares/subsystems/shared/imu_um6.makefile

@@ -0,0 +1,20 @@
+# Attitude estimation for fixedwings and rotorcrafts via CHR-UM6
+# 2012, Utah State University, http://aggieair.usu.edu/
+
+ifndef UM6_PORT
+  UM6_PORT=UART3
+endif
+ifndef UM6_BAUD
+  UM6_BAUD=B115200
+endif
+
+IMU_UM6_CFLAGS += -DUSE_IMU
+IMU_UM6_CFLAGS += -DIMU_TYPE_H=\"imu/imu_um6.h\"
+IMU_UM6_SRCS   += $(SRC_SUBSYSTEMS)/imu.c
+IMU_UM6_SRCS   += $(SRC_SUBSYSTEMS)/imu/imu_um6.c
+
+IMU_UM6_CFLAGS += -DUSE_$(UM6_PORT) -D$(UM6_PORT)_BAUD=$(UM6_BAUD)
+IMU_UM6_CFLAGS += -DUM6_LINK=$(UM6_PORT)
+
+ap.CFLAGS += $(IMU_UM6_CFLAGS)
+ap.srcs   += $(IMU_UM6_SRCS)

sw/airborne/subsystems/ahrs/ahrs_extern_quat.c

@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2013 Michal Podhradsky
+ * Utah State University, http://aggieair.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 subsystems/ahrs/ahrs_extern_quat.c
+ *
+ * AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc.
+ *
+ * Propagates the estimated attitude and rates from IMU to body states. Quaternion
+ * calculation is used.
+ *
+ * @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
+ */
+#include "ahrs_extern_quat.h"
+#include "mcu_periph/sys_time.h"
+#include "led.h"
+
+struct AhrsIntExternQuat ahrs_impl;
+
+void ahrs_init(void) {
+  ahrs.status = AHRS_UNINIT;
+
+  /* set ltp_to_imu so that body is zero */
+  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
+  INT_RATES_ZERO(ahrs_impl.imu_rate);
+
+  #ifdef IMU_MAG_OFFSET
+    ahrs_impl.mag_offset = IMU_MAG_OFFSET;
+  #else
+    ahrs_impl.mag_offset = 0.;
+  #endif
+
+  //Needed to set orientations
+  ahrs.status = AHRS_RUNNING;
+
+  #ifdef AHRS_ALIGNER_LED
+      LED_ON(AHRS_ALIGNER_LED);
+  #endif
+}
+
+void ahrs_propagate(void) {
+  /* Compute LTP to BODY quaternion */
+  struct Int32Quat ltp_to_body_quat;
+  INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
+  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
+
+  // TODO: compensate for magnetic offset
+
+  struct Int32Rates body_rate;
+  ahrs_impl.imu_rate = imu.gyro;
+  /* compute body rates */
+  INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate);
+  /* Set state */
+  stateSetBodyRates_i(&body_rate);
+}
+
+void ahrs_align(void) {
+}
+
+void ahrs_update_accel(void) {
+}
+
+void ahrs_update_mag(void) {
+}
+
+void ahrs_update_gps(void) {
+}

sw/airborne/subsystems/ahrs/ahrs_extern_quat.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2013 Michal Podhradsky
+ * Utah State University, http://aggieair.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 subsystems/ahrs/ahrs_extern_quat.h
+ *
+ * AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc.
+ *
+ * Propagates the estimated attitude and rates from IMU to body states. Quaternion
+ * calculation is used.
+ *
+ * @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
+ */
+#ifndef AHRS_EXTERN_QUAT_H
+#define AHRS_EXTERN_QUAT_H
+
+#include "state.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/imu.h"
+
+struct AhrsIntExternQuat {
+  struct Int32Eulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
+  struct Int32Quat   ltp_to_imu_quat;  ///< Rotation from LocalTangentPlane to IMU frame as quaternions
+  struct Int32Rates  imu_rate;         ///< Rotational velocity in IMU frame
+  float mag_offset;
+};
+
+extern struct AhrsIntExternQuat ahrs_impl;
+
+#endif /* AHRS_EXTERN_QUAT_H */