diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile
new file mode 100644
index 0000000000..2d0d4cee14
--- /dev/null
+++ b/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)
diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile
deleted file mode 100644
index 9a2087c811..0000000000
--- a/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile
+++ /dev/null
@@ -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)
diff --git a/conf/firmwares/subsystems/shared/ahrs_gx3.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile
similarity index 77%
rename from conf/firmwares/subsystems/shared/ahrs_gx3.makefile
rename to conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile
index 8ac0680b76..253b4c62a6 100644
--- a/conf/firmwares/subsystems/shared/ahrs_gx3.makefile
+++ b/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)
diff --git a/conf/messages.xml b/conf/messages.xml
index ab52141ce3..8a610824c1 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -603,7 +603,14 @@
-
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h
index c22d64d773..76bdf38f2b 100644
--- a/sw/airborne/firmwares/fixedwing/ap_downlink.h
+++ b/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 */
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 14674522cb..8c1e8bc7be 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/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
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 5d5ac26398..060367961d 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/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();
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 48a914b5f5..4522d049b4 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/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 */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c
deleted file mode 100644
index b778af0233..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c
+++ /dev/null
@@ -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
- */
-#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(<p_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) {
-}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h
deleted file mode 100644
index 74a415f387..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h
+++ /dev/null
@@ -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
- */
-#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 */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
index 2911d64150..4fa28e6f48 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
@@ -30,9 +30,19 @@
* @author Michal Podhradsky
*/
#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 180.0) {
+ course_f -= 360.0;
+ }
+ ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
+#endif
+ stateSetNedToBodyEulers_f(<p_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(<p_to_body_eulers);
+#else
+ stateSetNedToBodyRMat_f(<p_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) {
}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.h b/sw/airborne/subsystems/ahrs/ahrs_gx3.h
index e56cf0ed37..cf2c464f21 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_gx3.h
+++ b/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*/
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
index 74432019cb..3d9998bb99 100644
--- a/sw/airborne/subsystems/imu.c
+++ b/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);
}
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
index 72f759f7ef..1d082a9f58 100644
--- a/sw/airborne/subsystems/imu.h
+++ b/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