[ahrs] Upgraded GX3 ahrs subsystem

1) Separate makefiles for fixedwing and rotorcraft (so it can be used in both)
2) Configurable GX3 initialization
3) Acceleration and rates properly rotated into the body frame
4) uses float imu struct to keep the precision
5) adds GX3_INFO message
6) fixed IMU_GYRO and IMU_ACCEL messages for float imu struct
7) removed ahrs_extern_quat interface as obsolete

Especially the fixed point imu struct is used for backwards compatibility,
maybe there is a more elegant way how to use the floating point interface.

after review:

- changed UART macros for uart functions
- GX3 variables moved into ahrs struct
- rates and accel used in imu frame and stored in imu.f
- removed unnecessary math macros
- renamed variables to lowercase and removed unused ones
- removed RMAT to Quat conversion,
- using strictly RMAT and Eulers (if needed)
- better written calculation of gx3 frequency. It is actually more precise like this.

closes pull request #504

conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile

@@ -0,0 +1,30 @@
+# Fixedwing 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
+
+#fixedwings
+AHRS_CFLAGS += -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
+
+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
+
+GX3_PORT_LOWER=$(shell echo $(GX3_PORT) | tr A-Z a-z)
+AHRS_CFLAGS += -DUSE_$(GX3_PORT) -D$(GX3_PORT)_BAUD=$(GX3_BAUD)
+AHRS_CFLAGS += -DUSE_GX3 -DGX3_PORT=$(GX3_PORT_LOWER)
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)

conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile

@@ -1,17 +0,0 @@
-#
-# 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/rotorcraft/ahrs_gx3.makefile

@@ -1,4 +1,4 @@
-# AHRS module for GX3
+# Rotorcraft AHRS module for GX3
 # 2013, Utah State University, http://aggieair.usu.edu/
 
 GX3_PORT ?= UART3
@@ -7,6 +7,7 @@ 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)
@@ -17,8 +18,9 @@ AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs.c
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/imu.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_gx3.c
 
+GX3_PORT_LOWER=$(shell echo $(GX3_PORT) | tr A-Z a-z)
 AHRS_CFLAGS += -DUSE_$(GX3_PORT) -D$(GX3_PORT)_BAUD=$(GX3_BAUD)
-AHRS_CFLAGS += -DUSE_GX3 -DGX3_LINK=$(GX3_PORT)
+AHRS_CFLAGS += -DUSE_GX3 -DGX3_PORT=$(GX3_PORT_LOWER)
 
 ap.CFLAGS += $(AHRS_CFLAGS)
 ap.srcs += $(AHRS_SRCS)

conf/messages.xml

@@ -603,7 +603,14 @@
 
 
  <!-- 72 is free -->
- <!-- 73 is free -->
+
+  <message name="GX3_INFO" id="73">
+    <field name="GX3_freq" 	type="float" 	unit="hz"/>
+    <field name="chksm_error" 	type="uint32"/>
+    <field name="hdr_error" 	type="uint32"/>
+    <field name="GX3_chksm"	type="uint16"/>
+  </message>
+
  <!-- 74 is free -->
  <!-- 75 is free -->
 

sw/airborne/firmwares/fixedwing/ap_downlink.h

@@ -150,6 +150,11 @@
 
 #define PERIODIC_SEND_SEGMENT(_trans, _dev) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_trans, _dev, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
 
+#if USE_IMU_FLOAT
+#  include "subsystems/imu.h"
+#  define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &imuf.accel.x, &imuf.accel.y, &imuf.accel.z)}
+#  define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r)}
+#else
 #ifdef IMU_TYPE_H
 #  ifdef INS_MODULE_H
 #  include "modules/ins/ins_module.h"
@@ -180,6 +185,7 @@
 #    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {}
 #    define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
 #endif
+#endif
 
 #ifdef IMU_ANALOG
 #define PERIODIC_SEND_IMU(_trans, _dev) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_trans, _dev, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); }
@@ -317,5 +323,14 @@
 #include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
 #define PERIODIC_SEND_H_CTL_A(_trans, _dev) DOWNLINK_SEND_H_CTL_A(_trans, _dev, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
 
+#ifdef USE_GX3
+#define PERIODIC_SEND_GX3_INFO(_trans, _dev) DOWNLINK_SEND_GX3_INFO(_trans, _dev,\
+    &ahrs_impl.GX3_freq,			\
+    &ahrs_impl.GX3_packet.chksm_error,	\
+    &ahrs_impl.GX3_packet.hdr_error,	\
+    &ahrs_impl.GX3_chksm)
+#else
+#define PERIODIC_SEND_GX3_INFO(_trans, _dev) {}
+#endif
 
 #endif /* AP_DOWNLINK_H */

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -179,6 +179,9 @@ void init_ap( void ) {
 
 #if USE_IMU
   imu_init();
+#if USE_IMU_FLOAT
+  imu_float_init();
+#endif
 #endif
 
 #if USE_AHRS_ALIGNER
@@ -669,7 +672,6 @@ void event_task_ap( void ) {
   if (new_ins_attitude > 0)
   {
     attitude_loop();
-    //LED_TOGGLE(3);
     new_ins_attitude = 0;
   }
 #endif

sw/airborne/firmwares/rotorcraft/main.c

@@ -138,7 +138,9 @@ STATIC_INLINE void main_init( void ) {
 
   baro_init();
   imu_init();
-
+#if USE_IMU_FLOAT
+  imu_float_init();
+#endif
   ahrs_aligner_init();
   ahrs_init();
 

sw/airborne/firmwares/rotorcraft/telemetry.h

@@ -1010,4 +1010,14 @@
 #define PERIODIC_SEND_UART_ERRORS(_trans, _dev) {}
 #endif
 
+#ifdef USE_GX3
+#define PERIODIC_SEND_GX3_INFO(_trans, _dev) DOWNLINK_SEND_GX3_INFO(_trans, _dev,\
+    &ahrs_impl.GX3_freq,			\
+    &ahrs_impl.GX3_packet.chksm_error,	\
+    &ahrs_impl.GX3_packet.hdr_error,	\
+    &ahrs_impl.GX3_chksm)
+#else
+#define PERIODIC_SEND_GX3_INFO(_trans, _dev) {}
+#endif
+
 #endif /* TELEMETRY_H */

sw/airborne/subsystems/ahrs/ahrs_extern_quat.c

@@ -1,85 +0,0 @@
-/*
- * 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

@@ -1,48 +0,0 @@
-/*
- * 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 */

sw/airborne/subsystems/ahrs/ahrs_gx3.c

@@ -30,9 +30,19 @@
  * @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)
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+#endif
+
 #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
 
 /*
@@ -41,24 +51,10 @@
  * 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 bool_t gx3_verify_chk(volatile uint8_t *buff_add);
 static inline float bef(volatile uint8_t *c);
 
 /* Big Endian to Float */
@@ -73,7 +69,7 @@ static inline float bef(volatile uint8_t *c) {
   return f;
 }
 
-static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add) {
+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++) {
@@ -83,223 +79,255 @@ static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add) {
 }
 
 void ahrs_align(void) {
-  GX3_status = GX3Uninit;
+  ahrs_impl.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));
+  uart_transmit(&GX3_PORT, 0xcd);
+  uart_transmit(&GX3_PORT, 0xc1);
+  uart_transmit(&GX3_PORT, 0x29);
+  uart_transmit(&GX3_PORT, 0x27);
+  uart_transmit(&GX3_PORT, 0x10);
 
-  GX3_status = GX3Running;
+  ahrs_impl.gx3_status = GX3Running;
 }
 
-
+/*
+ * GX3 can be set up during the startup, or it can be configured to
+ * start sending data automatically after power up.
+ */
 void imu_impl_init(void) {
   // Initialize variables
-  GX3_status = GX3Uninit;
+  ahrs_impl.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;
+  ahrs_impl.gx3_packet.status = GX3PacketWaiting;
+  ahrs_impl.gx3_packet.msg_idx = 0;
+  ahrs_impl.gx3_packet.msg_available = FALSE;
+  ahrs_impl.gx3_packet.chksm_error = 0;
+  ahrs_impl.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");
   }
 
+#ifdef GX3_INITIALIZE_DURING_STARTUP
+#pragma message "GX3 initializing"
   /*
   // 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
+  uart_transmit(&GX3_PORT, 0xc4);
+  uart_transmit(&GX3_PORT, 0xc1);
+  uart_transmit(&GX3_PORT, 0x29);
+  uart_transmit(&GX3_PORT, 0x00); // stop
   */
 
   //Sampling Settings (0xDB)
-  GX3Link(Transmit(0xdb)); //set update speed
-  GX3Link(Transmit(0xa8));
-  GX3Link(Transmit(0xb9));
+  uart_transmit(&GX3_PORT, 0xdb); //set update speed
+  uart_transmit(&GX3_PORT, 0xa8);
+  uart_transmit(&GX3_PORT, 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
+#ifdef GX3_SAVE_SETTINGS
+  uart_transmit(&GX3_PORT, 0x02);//set params and save them in non-volatile memory
+#else
+  uart_transmit(&GX3_PORT, 0x02); //set and don't save
+#endif
+  uart_transmit(&GX3_PORT, IMU_DIV1);
+  uart_transmit(&GX3_PORT, IMU_DIV2);
+  uart_transmit(&GX3_PORT, 0b00000000);  //set options byte 8 - GOOD
+  uart_transmit(&GX3_PORT, 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
+  // 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));
+  uart_transmit(&GX3_PORT, ACC_FILT_DIV);
+  uart_transmit(&GX3_PORT, MAG_FILT_DIV); //mag window filter size == 33hz
+  uart_transmit(&GX3_PORT, 0x00);
+  uart_transmit(&GX3_PORT, 10); // Up Compensation in secs, def=10s
+  uart_transmit(&GX3_PORT, 0x00);
+  uart_transmit(&GX3_PORT, 10); // North Compensation in secs
+  uart_transmit(&GX3_PORT, 0x00); //power setting = 0, high power/bw
+  uart_transmit(&GX3_PORT, 0x00); //rest of the bytes are 0
+  uart_transmit(&GX3_PORT, 0x00);
+  uart_transmit(&GX3_PORT, 0x00);
+  uart_transmit(&GX3_PORT, 0x00);
+  uart_transmit(&GX3_PORT, 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));
+    uart_transmit(&GX3_PORT, 0xdd);
+    uart_transmit(&GX3_PORT, 0x54);
+    uart_transmit(&GX3_PORT, 0x4c);
+    uart_transmit(&GX3_PORT, 3);
+    uart_transmit(&GX3_PORT, 10);
+    uart_transmit(&GX3_PORT, 10);
+    uart_transmit(&GX3_PORT, 0x00);
+    uart_transmit(&GX3_PORT, 0x00);
+    uart_transmit(&GX3_PORT, 0x00);
+    uart_transmit(&GX3_PORT, 0x00);
   */
 
-  // Another wait loop for proper GX3 init
+  //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
+#ifdef GX3_SET_WAKEUP_MODE
+  //Mode Preset (0xD5)
+  uart_transmit(&GX3_PORT, 0xD5);
+  uart_transmit(&GX3_PORT, 0xBA);
+  uart_transmit(&GX3_PORT, 0x89);
+  uart_transmit(&GX3_PORT, 0x02); // wake up in continuous mode
 
-  // Reset gyros to zerp
+  //Continuous preset (0xD6)
+  uart_transmit(&GX3_PORT, 0xD6);
+  uart_transmit(&GX3_PORT, 0xC6);
+  uart_transmit(&GX3_PORT, 0x6B);
+  uart_transmit(&GX3_PORT, 0xc8); // accel, gyro, R
+#endif
+
+  //4 byte command for Continous Mode
+  uart_transmit(&GX3_PORT, 0xc4);
+  uart_transmit(&GX3_PORT, 0xc1);
+  uart_transmit(&GX3_PORT, 0x29);
+  uart_transmit(&GX3_PORT, 0xc8); // accel,gyro,R
+
+  // Reset gyros to zero
   ahrs_align();
+#endif
+  ahrs.status = AHRS_RUNNING;
 }
 
 
 void imu_periodic(void) {
   /* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW
-     GX3Link(Transmit(0xc8)); // accel,gyro,R
+     uart_transmit(&GX3_PORT, 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;
+void gx3_packet_read_message(void) {
+  ahrs_impl.gx3_accel.x     = bef(&ahrs_impl.gx3_packet.msg_buf[1]);
+  ahrs_impl.gx3_accel.y     = bef(&ahrs_impl.gx3_packet.msg_buf[5]);
+  ahrs_impl.gx3_accel.z     = bef(&ahrs_impl.gx3_packet.msg_buf[9]);
+  ahrs_impl.gx3_rate.p      = bef(&ahrs_impl.gx3_packet.msg_buf[13]);
+  ahrs_impl.gx3_rate.q      = bef(&ahrs_impl.gx3_packet.msg_buf[17]);
+  ahrs_impl.gx3_rate.r      = bef(&ahrs_impl.gx3_packet.msg_buf[21]);
+  ahrs_impl.gx3_rmat.m[0]   = bef(&ahrs_impl.gx3_packet.msg_buf[25]);
+  ahrs_impl.gx3_rmat.m[1]   = bef(&ahrs_impl.gx3_packet.msg_buf[29]);
+  ahrs_impl.gx3_rmat.m[2]   = bef(&ahrs_impl.gx3_packet.msg_buf[33]);
+  ahrs_impl.gx3_rmat.m[3]   = bef(&ahrs_impl.gx3_packet.msg_buf[37]);
+  ahrs_impl.gx3_rmat.m[4]   = bef(&ahrs_impl.gx3_packet.msg_buf[41]);
+  ahrs_impl.gx3_rmat.m[5]   = bef(&ahrs_impl.gx3_packet.msg_buf[45]);
+  ahrs_impl.gx3_rmat.m[6]   = bef(&ahrs_impl.gx3_packet.msg_buf[49]);
+  ahrs_impl.gx3_rmat.m[7]   = bef(&ahrs_impl.gx3_packet.msg_buf[53]);
+  ahrs_impl.gx3_rmat.m[8]   = bef(&ahrs_impl.gx3_packet.msg_buf[57]);
+  ahrs_impl.gx3_time    = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 |
+                                     ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]);
+  ahrs_impl.gx3_chksm   = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf);
 
-  GX3_freq = ((GX3_time - GX3_ltime))/16000000.0;
-  GX3_freq = 1.0/GX3_freq;
-  GX3_ltime = GX3_time;
+  ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
+  ahrs_impl.gx3_ltime = ahrs_impl.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;
+  VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
+  ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
+  imuf.accel = ahrs_impl.gx3_accel;
 
   // Rates
   struct FloatRates body_rate;
-  ahrs_impl.imu_rate = GX3_rate;
+  imuf.gyro = ahrs_impl.gx3_rate;
   /* compute body rates */
-  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, imuf.body_to_imu_rmat, ahrs_impl.imu_rate);
+  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, imuf.body_to_imu_rmat, imuf.gyro);
   /* 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
+  // Attitude
+  struct FloatRMat ltp_to_body_rmat;
+  FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, imuf.body_to_imu_rmat);
+#ifdef AHRS_UPDATE_FW_ESTIMATOR // fixedwing
+  struct FloatEulers ltp_to_body_eulers;
+  FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat);
+  ltp_to_body_eulers.phi -= ins_roll_neutral;
+  ltp_to_body_eulers.theta -= ins_pitch_neutral;
+#if AHRS_USE_GPS_HEADING && USE_GPS
+  float course_f = (float)DegOfRad(gps.course / 1e7);
+  if (course_f > 180.0) {
+    course_f -= 360.0;
+  }
+  ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
+#endif
+  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
+#else
+#ifdef IMU_MAG_OFFSET //rotorcraft
+  struct FloatEulers ltp_to_body_eulers;
+  FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat);
+  ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
+  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
+#else
+  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
+#endif
+#endif
 }
 
 
 /* GX3 Packet Collection */
-void GX3_packet_parse( uint8_t c ) {
-  switch (GX3_packet.status) {
+void gx3_packet_parse( uint8_t c ) {
+  switch (ahrs_impl.gx3_packet.status) {
     case GX3PacketWaiting:
-      GX3_packet.msg_idx = 0;
+      ahrs_impl.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++;
+        ahrs_impl.gx3_packet.status++;
+        ahrs_impl.gx3_packet.msg_buf[ahrs_impl.gx3_packet.msg_idx] = c;
+        ahrs_impl.gx3_packet.msg_idx++;
       } else {
-        GX3_packet.hdr_error++;
+        ahrs_impl.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;
+      ahrs_impl.gx3_packet.msg_buf[ahrs_impl.gx3_packet.msg_idx] =  c;
+      ahrs_impl.gx3_packet.msg_idx++;
+      if (ahrs_impl.gx3_packet.msg_idx == GX3_MSG_LEN) {
+        if (gx3_verify_chk(ahrs_impl.gx3_packet.msg_buf)) {
+          ahrs_impl.gx3_packet.msg_available = TRUE;
         } else {
-          GX3_packet.msg_available = FALSE;
-          GX3_packet.chksm_error++;
+          ahrs_impl.gx3_packet.msg_available = FALSE;
+          ahrs_impl.gx3_packet.chksm_error++;
         }
-        GX3_packet.status = 0;
+        ahrs_impl.gx3_packet.status = 0;
       }
       break;
     default:
-      GX3_packet.status = 0;
-      GX3_packet.msg_idx = 0;
+      ahrs_impl.gx3_packet.status = 0;
+      ahrs_impl.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.;
+  ahrs_impl.mag_offset = 0.0;
 #endif
-
   ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
 }
 
 void ahrs_aligner_run(void) {
 #ifdef AHRS_ALIGNER_LED
-  LED_TOGGLE(AHRS_ALIGNER_LED);
+  LED_ON(AHRS_ALIGNER_LED);
 #endif
-
-  if (GX3_freq > GX3_MIN_FREQ) {
-    ahrs.status = AHRS_RUNNING;
-#ifdef AHRS_ALIGNER_LED
-    LED_ON(AHRS_ALIGNER_LED);
-#endif
-  }
+  ahrs.status = AHRS_RUNNING;
 }
 
+
 void ahrs_aligner_init(void) {
 }
 

sw/airborne/subsystems/ahrs/ahrs_gx3.h

@@ -36,18 +36,13 @@
 #include "subsystems/imu.h"
 #include "subsystems/ahrs.h"
 #include "subsystems/ins.h"
+#include "subsystems/gps.h"
 #include "mcu_periph/uart.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
 
 #include "state.h"
 #include "led.h"
 
-#define __GX3Link(dev, _x) dev##_x
-#define _GX3Link(dev, _x)  __GX3Link(dev, _x)
-#define GX3Link(_x) _GX3Link(GX3_LINK, _x)
-
-#define GX3Buffer() GX3Link(ChAvailable())
-
 #ifdef ImuScaleGyro
 #undef ImuScaleGyro
 #endif
@@ -70,18 +65,10 @@
 
 #define IMU_GX3_LONG_DELAY 4000000
 
-extern struct GX3_packet GX3_packet;
-extern enum GX3Status GX3_status;
+extern void gx3_packet_read_message(void);
+extern void gx3_packet_parse(uint8_t c);
 
-extern void GX3_packet_read_message(void);
-extern void GX3_packet_parse(uint8_t c);
-
-extern float GX3_freq;
-extern uint16_t GX3_chksm;
-extern uint16_t GX3_calcsm;
-extern uint32_t GX3_time;
-
-struct GX3_packet {
+struct GX3Packet {
   bool_t  msg_available;
   uint32_t chksm_error;
   uint32_t hdr_error;
@@ -90,7 +77,7 @@ struct GX3_packet {
   uint8_t  msg_idx;
 };
 
-enum GX36PacketStatus {
+enum GX3PacketStatus {
   GX3PacketWaiting,
   GX3PacketReading
 };
@@ -102,30 +89,43 @@ enum GX3Status {
 
 //AHRS
 struct AhrsFloatQuat {
-  struct FloatEulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
   struct FloatQuat   ltp_to_imu_quat;  ///< Rotation from LocalTangentPlane to IMU frame as quaternions
-  struct FloatRates  imu_rate;         ///< Rotational velocity in IMU frame
-  float mag_offset;
+  float mag_offset;                    ///< Difference between true and magnetic north
+
+  struct GX3Packet gx3_packet;       ///< Packet struct
+  enum GX3Status gx3_status;          ///< GX3 status
+  float gx3_freq;                     ///< data frequency
+  uint16_t gx3_chksm;                 ///< aux variable for checksum
+  uint32_t gx3_time;                  ///< GX3 time stamp
+  uint32_t gx3_ltime;                 ///< aux time stamp
+  struct FloatVect3 gx3_accel;        ///< measured acceleration in IMU frame
+  struct FloatRates gx3_rate;         ///< measured angular rates in IMU frame
+  struct FloatRMat  gx3_rmat;         ///< measured attitude in IMU frame (rotational matrix)
 };
 
 extern struct AhrsFloatQuat ahrs_impl;
 
 static inline void ReadGX3Buffer(void) {
-  while (GX3Link(ChAvailable()) && !GX3_packet.msg_available)
-    GX3_packet_parse(GX3Link(Getch()));
+  while (uart_char_available(&GX3_PORT) && !ahrs_impl.gx3_packet.msg_available)
+    gx3_packet_parse(uart_getch(&GX3_PORT));
 }
 
 static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
-  if (GX3Buffer()) {
+  if (uart_char_available(&GX3_PORT)) {
     ReadGX3Buffer();
   }
-  if (GX3_packet.msg_available) {
-    GX3_packet_read_message();
+  if (ahrs_impl.gx3_packet.msg_available) {
+    gx3_packet_read_message();
     _gyro_handler();
     _accel_handler();
     _mag_handler();
-    GX3_packet.msg_available = FALSE;
+    ahrs_impl.gx3_packet.msg_available = FALSE;
   }
 }
 
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+#endif
+
 #endif /* AHRS_GX3_H*/

sw/airborne/subsystems/imu.c

@@ -27,6 +27,7 @@
 #include "subsystems/imu.h"
 
 struct Imu imu;
+struct ImuFloat imuf;
 
 void imu_init(void) {
 
@@ -60,16 +61,14 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!")
 }
 
 
-void imu_float_init(struct ImuFloat* imuf) {
-
+void imu_float_init(void) {
   /*
     Compute quaternion and rotation matrix
     for conversions between body and imu frame
   */
-  EULERS_ASSIGN(imuf->body_to_imu_eulers,
+  EULERS_ASSIGN(imuf.body_to_imu_eulers,
 		IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
-  FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
-  FLOAT_QUAT_NORMALIZE(imuf->body_to_imu_quat);
-  FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
-
+  FLOAT_QUAT_OF_EULERS(imuf.body_to_imu_quat, imuf.body_to_imu_eulers);
+  FLOAT_QUAT_NORMALIZE(imuf.body_to_imu_quat);
+  FLOAT_RMAT_OF_EULERS(imuf.body_to_imu_rmat, imuf.body_to_imu_eulers);
 }

sw/airborne/subsystems/imu.h

@@ -64,10 +64,11 @@ struct ImuFloat {
   uint32_t sample_count;
 };
 
-extern void imu_float_init(struct ImuFloat* imuf);
+
 
 /** global IMU state */
 extern struct Imu imu;
+extern struct ImuFloat imuf;
 
 /* underlying hardware */
 #ifdef IMU_TYPE_H
@@ -75,7 +76,7 @@ extern struct Imu imu;
 #endif
 
 extern void imu_init(void);
-
+extern void imu_float_init(void);
 
 #if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
 #define IMU_BODY_TO_IMU_PHI   0