[ahrs] Add Microstrain GX3 AHRS subystem

The AP stays in kill mode until it gets valid data from AHRS (using ahrs_aligner)

conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile

@@ -0,0 +1,25 @@
+# AHRS module for GX3
+# 2013, Utah State University, http://aggieair.usu.edu/
+
+GX3_PORT ?= UART3
+GX3_BAUD?=B921600
+
+AHRS_CFLAGS  = -DUSE_AHRS
+AHRS_CFLAGS += -DUSE_IMU
+AHRS_CFLAGS += -DUSE_IMU_FLOAT
+AHRS_CFLAGS += -DUSE_GX3
+
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
+AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs.c
+AHRS_SRCS   += $(SRC_SUBSYSTEMS)/imu.c     
+AHRS_SRCS   += subsystems/ahrs/ahrs_gx3.c
+
+AHRS_CFLAGS += -DUSE_$(GX3_PORT) -D$(GX3_PORT)_BAUD=$(GX3_BAUD)
+AHRS_CFLAGS += -DUSE_GX3 -DGX3_LINK=$(GX3_PORT) 
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)

sw/airborne/subsystems/ahrs/ahrs_gx3.c

@@ -0,0 +1,316 @@
+/*
+ * 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 ahrs_gx3.c
+ *
+ * Driver for Microstrain GX3 IMU/AHRS subsystem
+ *
+ * Takes care of configuration of the IMU, communication and parsing
+ * the received packets. See GX3 datasheet for configuration options.
+ *
+ * @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
+ */
+#include "subsystems/ahrs/ahrs_gx3.h"
+#include "mcu_periph/sys_time.h"
+
+#define GX3_TIME(_ubx_payload) (uint32_t)((uint32_t)(*((uint8_t*)_ubx_payload+62+3))|(uint32_t)(*((uint8_t*)_ubx_payload+62+2))<<8|(uint32_t)(*((uint8_t*)_ubx_payload+62+1))<<16|(uint32_t)(*((uint8_t*)_ubx_payload+62+0))<<24)
+#define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
+
+/*
+ * Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
+ * Positive pitch : nose up
+ * Positive roll : right wing down
+ * Positive yaw : clockwise
+ */
+struct GX3_packet GX3_packet;
+enum GX3Status GX3_status;
+uint32_t GX3_time;
+uint32_t GX3_ltime;
+uint16_t GX3_chksm;
+uint16_t GX3_calcsm;
+float GX3_freq;
+
+struct FloatVect3 GX3_accel;
+struct FloatRates GX3_rate;
+struct FloatRMat  GX3_rmat;
+struct FloatQuat GX3_quat;
+struct FloatEulers GX3_euler;
+
+struct AhrsFloatQuat ahrs_impl;
+struct AhrsAligner ahrs_aligner;
+
+static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add);
+static inline float bef(volatile uint8_t *c);
+
+/* Big Endian to Float */
+static inline float bef(volatile uint8_t *c) { 
+    float f;
+    int8_t * p;
+    p = ((int8_t *)&f)+3;
+    *p-- = *c++;
+    *p-- = *c++;
+    *p-- = *c++;
+    *p = *c;
+    return f;
+}
+
+static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add) {
+    uint16_t i,chk_calc;
+    chk_calc = 0;
+    for (i=0;i<GX3_MSG_LEN-2;i++) {
+        chk_calc += (uint8_t)*buff_add++;
+    }
+    return (chk_calc == ( (((uint16_t)*buff_add)<<8) + (uint8_t)*(buff_add+1) ));
+}
+
+void ahrs_align(void) {
+   GX3_status = GX3Uninit;
+
+   //make the gyros zero, takes 10s (specified in Byte 4 and 5)
+   GX3Link(Transmit(0xcd));
+   GX3Link(Transmit(0xc1));
+   GX3Link(Transmit(0x29));
+   GX3Link(Transmit(0x27));
+   GX3Link(Transmit(0x10));
+
+   GX3_status = GX3Running;
+}
+
+
+void imu_impl_init(void) { 
+  // Initialize variables
+  GX3_status = GX3Uninit;
+
+  // Initialize packet
+  GX3_packet.status = GX3PacketWaiting;
+  GX3_packet.msg_idx = 0;
+  GX3_packet.msg_available = FALSE;
+  GX3_packet.chksm_error = 0;
+  GX3_packet.hdr_error = 0;
+
+  // It is necessary to wait for GX3 to power up for proper initialization
+  for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY*2; startup_counter++){
+    __asm("nop");
+  }
+
+/* 
+  // FOR NON-CONTINUOUS MODE UNCOMMENT THIS
+  //4 byte command for non-Continous Mode so we can set the other settings
+   GX3Link(Transmit(0xc4));
+   GX3Link(Transmit(0xc1));
+   GX3Link(Transmit(0x29));
+   GX3Link(Transmit(0x00)); // stop
+*/
+
+   //Sampling Settings (0xDB)
+   GX3Link(Transmit(0xdb)); //set update speed
+   GX3Link(Transmit(0xa8));
+   GX3Link(Transmit(0xb9));
+   //set rate of IMU link, is 1000/IMU_DIV
+   #define IMU_DIV1 0
+   #define IMU_DIV2 2
+   #define ACC_FILT_DIV 2 
+   #define MAG_FILT_DIV 30 
+   GX3Link(Transmit(0x01));//set params, don't store
+   GX3Link(Transmit(IMU_DIV1));
+   GX3Link(Transmit(IMU_DIV2));
+   GX3Link(Transmit(0b00000000));  //set options byte 8 - GOOD
+   GX3Link(Transmit(0b00000011));  //set options byte 7 - GOOD
+   //0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data, 
+   // 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved, 
+   // 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation
+   // 12 - no quaternion calculation, 13-15 reserved
+   GX3Link(Transmit(ACC_FILT_DIV));
+   GX3Link(Transmit(MAG_FILT_DIV)); //mag window filter size == 33hz
+   GX3Link(Transmit(0x00));
+   GX3Link(Transmit(10)); // Up Compensation in secs, def=10s
+   GX3Link(Transmit(0x00));
+   GX3Link(Transmit(10)); // North Compensation in secs
+   GX3Link(Transmit(0x00)); //power setting = 0, high power/bw
+   GX3Link(Transmit(0x00)); //rest of the bytes are 0
+   GX3Link(Transmit(0x00));
+   GX3Link(Transmit(0x00));
+   GX3Link(Transmit(0x00));
+   GX3Link(Transmit(0x00));
+
+  // OPTIONAL: realign up and north
+  /*
+  GX3Link(Transmit(0xdd));
+  GX3Link(Transmit(0x54));
+  GX3Link(Transmit(0x4c));
+  GX3Link(Transmit(3));
+  GX3Link(Transmit(10));
+  GX3Link(Transmit(10));
+  GX3Link(Transmit(0x00));
+  GX3Link(Transmit(0x00));
+  GX3Link(Transmit(0x00));
+  GX3Link(Transmit(0x00));
+  */
+
+  // Another wait loop for proper GX3 init
+  for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY; startup_counter++){
+    __asm("nop");
+  }
+
+  //4 byte command for Continous Mode  
+   GX3Link(Transmit(0xc4));
+   GX3Link(Transmit(0xc1));
+   GX3Link(Transmit(0x29));
+   GX3Link(Transmit(0xc8)); // accel,gyro,R
+
+  // Reset gyros to zerp
+  ahrs_align();
+}
+
+
+void imu_periodic(void) {
+  /* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW 
+  GX3Link(Transmit(0xc8)); // accel,gyro,R
+  */
+}
+
+
+void GX3_packet_read_message(void) {
+    GX3_accel.x 	= bef(&GX3_packet.msg_buf[1]);
+    GX3_accel.y 	= bef(&GX3_packet.msg_buf[5]);
+    GX3_accel.z 	= bef(&GX3_packet.msg_buf[9]);
+    GX3_rate.p  	= bef(&GX3_packet.msg_buf[13]);
+    GX3_rate.q  	= bef(&GX3_packet.msg_buf[17]);
+    GX3_rate.r  	= bef(&GX3_packet.msg_buf[21]);
+    GX3_rmat.m[0] 	= bef(&GX3_packet.msg_buf[25]);
+    GX3_rmat.m[1] 	= bef(&GX3_packet.msg_buf[29]);
+    GX3_rmat.m[2] 	= bef(&GX3_packet.msg_buf[33]);
+    GX3_rmat.m[3] 	= bef(&GX3_packet.msg_buf[37]);
+    GX3_rmat.m[4] 	= bef(&GX3_packet.msg_buf[41]);
+    GX3_rmat.m[5] 	= bef(&GX3_packet.msg_buf[45]);
+    GX3_rmat.m[6] 	= bef(&GX3_packet.msg_buf[49]);
+    GX3_rmat.m[7] 	= bef(&GX3_packet.msg_buf[53]);
+    GX3_rmat.m[8] 	= bef(&GX3_packet.msg_buf[57]);
+	  GX3_time 	= GX3_TIME(GX3_packet.msg_buf);
+	  GX3_chksm	= GX3_CHKSM(GX3_packet.msg_buf);
+    GX3_calcsm = 0;
+
+    GX3_freq = ((GX3_time - GX3_ltime))/16000000.0;
+	  GX3_freq = 1.0/GX3_freq;
+    GX3_ltime = GX3_time;
+
+    // Acceleration 
+    VECT3_SMUL(GX3_accel, GX3_accel, 9.80665); // Convert g into m/s2
+    ACCELS_BFP_OF_REAL(imu.accel, GX3_accel);
+    imuf.accel = GX3_accel;
+
+	  // Rates 
+  	struct FloatRates body_rate;
+    ahrs_impl.imu_rate = GX3_rate;
+    /* compute body rates */
+    FLOAT_RMAT_TRANSP_RATEMULT(body_rate, imuf.body_to_imu_rmat, ahrs_impl.imu_rate);
+    /* Set state */
+    stateSetBodyRates_f(&body_rate);
+
+    // Quaternions from rotation matrix 
+    FLOAT_QUAT_OF_RMAT(GX3_quat, GX3_rmat);
+    ahrs_impl.ltp_to_imu_quat = GX3_quat;
+    /* Compute LTP to BODY quaternion */
+    struct FloatQuat ltp_to_body_quat;
+    FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imuf.body_to_imu_quat);
+    stateSetNedToBodyQuat_f(&ltp_to_body_quat);
+
+    // TODO: compensate for magnetic offset
+}
+
+
+/* GX3 Packet Collection */
+void GX3_packet_parse( uint8_t c ) {
+  switch (GX3_packet.status) {
+  case GX3PacketWaiting:
+    GX3_packet.msg_idx = 0;
+    if (c == GX3_HEADER) {
+      GX3_packet.status++;
+      GX3_packet.msg_buf[GX3_packet.msg_idx] = c;
+      GX3_packet.msg_idx++;
+    } else {
+      GX3_packet.hdr_error++;
+    }
+    break;
+  case GX3PacketReading: 
+    GX3_packet.msg_buf[GX3_packet.msg_idx] =  c;
+    GX3_packet.msg_idx++;
+    if (GX3_packet.msg_idx == GX3_MSG_LEN) {
+      if (GX3_verify_chk(GX3_packet.msg_buf)) {
+        GX3_packet.msg_available = TRUE;
+      } else {
+        GX3_packet.msg_available = FALSE;
+        GX3_packet.chksm_error++;
+      }
+    GX3_packet.status = 0;
+    }
+    break;
+  default:
+    GX3_packet.status = 0;
+    GX3_packet.msg_idx = 0;
+    break;
+  }
+}
+
+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, imuf.body_to_imu_quat);
+  FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
+
+  #ifdef IMU_MAG_OFFSET
+    ahrs_impl.mag_offset = IMU_MAG_OFFSET;
+  #else
+    ahrs_impl.mag_offset = 0.;
+  #endif
+
+  ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
+}
+
+void ahrs_aligner_run(void) {
+  #ifdef AHRS_ALIGNER_LED
+    LED_TOGGLE(AHRS_ALIGNER_LED);
+  #endif
+
+  if (GX3_freq > GX3_MIN_FREQ) {
+    ahrs.status = AHRS_RUNNING;
+    #ifdef AHRS_ALIGNER_LED
+      LED_ON(AHRS_ALIGNER_LED);
+    #endif
+  }
+}
+
+void ahrs_aligner_init(void) {
+}
+
+void ahrs_propagate(void) {
+}
+
+void ahrs_update_accel(void) {
+}
+
+void ahrs_update_mag(void) {
+}
+
+void ahrs_update_gps(void) {
+}