[ahrs] remove AHRS_UPDATE_FW_ESTIMATOR and ins_x_neutrals

paparazzi · Aug 28, 2014 · 23a5e0e · 23a5e0e
1 parent 8fbd83d
commit 23a5e0e
Show file tree

Hide file tree

Showing 25 changed files with 26 additions and 218 deletions.
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
@@ -8,7 +8,7 @@
 USE_MAGNETOMETER ?= 0
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
@@ -48,7 +48,7 @@ nps.srcs += $(AHRS_SRCS)
 # Simple simulation of the AHRS result
 #
 ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ahrssim_CFLAGS += -DUSE_AHRS
 
 ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
 ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
@@ -8,7 +8,7 @@
 USE_MAGNETOMETER ?= 0
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
@@ -40,7 +40,7 @@ nps.srcs += $(AHRS_SRCS)
 # Simple simulation of the AHRS result
 #
 ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ahrssim_CFLAGS += -DUSE_AHRS
 
 ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
 ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile
@@ -7,7 +7,7 @@ AHRS_ALIGNER_LED ?= none
 
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
-AHRS_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS += -DUSE_AHRS
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
 AHRS_CFLAGS += -DUSE_MAGNETOMETER
@@ -36,7 +36,7 @@ nps.srcs += $(AHRS_SRCS)
 # Simple simulation of the AHRS result
 #
 ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ahrssim_CFLAGS += -DUSE_AHRS
 
 ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
 ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile
@@ -10,8 +10,6 @@ AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_IMU
 AHRS_CFLAGS += -DUSE_IMU_FLOAT
 
-#fixedwings
-AHRS_CFLAGS += -DAHRS_UPDATE_FW_ESTIMATOR
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 
 ifneq ($(AHRS_ALIGNER_LED),none)

diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
@@ -9,7 +9,7 @@
 USE_MAGNETOMETER ?= 0
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
@@ -49,7 +49,7 @@ nps.srcs += $(AHRS_SRCS)
 # Simple simulation of the AHRS result
 #
 ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ahrssim_CFLAGS += -DUSE_AHRS
 
 ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
 ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile b/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile
@@ -37,7 +37,7 @@ nps.srcs += $(INS_SRCS)
 # Simple simulation of the AHRS result
 #
 ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ahrssim_CFLAGS += -DUSE_AHRS
 
 ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
 ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
@@ -52,7 +52,7 @@ SIM_TARGETS = sim jsbsim nps
 ifneq (,$(findstring $(TARGET),$(SIM_TARGETS)))
 
 $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-$(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+$(TARGET).CFLAGS += -DUSE_AHRS
 
 $(TARGET).srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
 $(TARGET).srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile
diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile
@@ -6,6 +6,7 @@
 #
 
 USE_MAGNETOMETER ?= 1
+AHRS_ALIGNER_LED ?= none
 
 AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER

diff --git a/...ms/fixedwing/ahrs_int_cmpl_euler.makefile → ...stems/shared/ahrs_int_cmpl_euler.makefile b/...ms/fixedwing/ahrs_int_cmpl_euler.makefile → ...stems/shared/ahrs_int_cmpl_euler.makefile
@@ -1,10 +1,12 @@
 # Hey Emacs, this is a -*- makefile -*-
 #
+# Fixed point complementary filter using euler angles for attitude estimation
+#
 
 USE_MAGNETOMETER ?= 1
 AHRS_ALIGNER_LED ?= none
 
-AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 
 ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
@@ -30,7 +32,7 @@ nps.srcs += $(AHRS_SRCS)
 # Simple simulation of the AHRS result
 #
 ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ahrssim_CFLAGS += -DUSE_AHRS
 
 ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
 ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
@@ -94,18 +94,6 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
 #endif
 
-#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 //AHRS_UPDATE_FW_ESTIMATOR
-
 
 void ahrs_update_mag_full(void);
 void ahrs_update_mag_2d(void);
@@ -573,16 +561,7 @@ static inline void compute_body_orientation_and_rates(void) {
   struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
   FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
   /* Set state */
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-  struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. };
-  struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat;
-  FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
-  FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat);
-  FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
-  stateSetNedToBodyQuat_f(&ltp_to_neutrals_quat);
-#else
   stateSetNedToBodyQuat_f(&ltp_to_body_quat);
-#endif
 
   /* compute body rates */
   struct FloatRates body_rate;

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
@@ -74,10 +74,5 @@ void ahrs_update_heading(float heading);
  */
 void ahrs_realign_heading(float heading);
 
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-extern float ins_roll_neutral;
-extern float ins_pitch_neutral;
-#endif
-
 
 #endif /* AHRS_FLOAT_CMPL_RMAT */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -58,17 +58,6 @@
 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
 #endif
 
-// FIXME this is still needed for fixedwing integration
-// 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;
-
 
 struct AhrsFloatDCM ahrs_impl;
 
@@ -538,15 +527,7 @@ static inline void set_body_orientation_and_rates(void) {
   FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
   FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat);
 
-  // Some stupid lines of code for neutrals
-  struct FloatEulers ltp_to_body_euler;
-  FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
-  ltp_to_body_euler.phi -= ins_roll_neutral;
-  ltp_to_body_euler.theta -= ins_pitch_neutral;
-  stateSetNedToBodyEulers_f(&ltp_to_body_euler);
-
-  // should be replaced at the end by:
-  //   stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
+  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
 
 }
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -43,11 +43,6 @@ struct AhrsFloatDCM {
 extern struct AhrsFloatDCM ahrs_impl;
 
 
-// FIXME neutrals should be a feature of state interface ?
-extern float ins_roll_neutral;
-extern float ins_pitch_neutral;
-
-
 // DCM Parameters
 
 //#define Kp_ROLLPITCH 0.2

diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
@@ -31,17 +31,6 @@
  */
 #include "subsystems/ahrs/ahrs_gx3.h"
 
-#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)
 
@@ -266,29 +255,26 @@ void gx3_packet_read_message(void) {
   // Attitude
   struct FloatRMat ltp_to_body_rmat;
   FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);
-#ifdef AHRS_UPDATE_FW_ESTIMATOR // fixedwing
+
+#if AHRS_USE_GPS_HEADING && USE_GPS
   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
+#else // !AHRS_USE_GPS_HEADING
+#ifdef IMU_MAG_OFFSET
   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
+#endif // IMU_MAG_OFFSET
+#endif // !AHRS_USE_GPS_HEADING
 }
 
 

diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.h b/sw/airborne/subsystems/ahrs/ahrs_gx3.h
@@ -123,9 +123,4 @@ static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler
   }
 }
 
-#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/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
@@ -32,14 +32,6 @@
 #include "subsystems/ahrs.h"
 #include "std.h"
 
-/**
- * @todo harmonize infrared neutrals with ins_neutrals
- * or get rid of ins neutrals
- * this ins only needed for sim right now
- */
-extern float ins_roll_neutral;
-extern float ins_pitch_neutral;
-
 extern void ahrs_update_infrared(void);
 
 #endif /* AHRS_INFRARED_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
@@ -46,19 +46,6 @@
 #define AHRS_MAG_OFFSET 0.
 #endif
 
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-// remotely settable (for FW)
-#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
-
-
 struct AhrsIntCmplEuler ahrs_impl;
 
 static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel);
@@ -329,15 +316,7 @@ static void set_body_state_from_euler(void) {
   /* Compute LTP to BODY rotation matrix */
   INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat);
   /* Set state */
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-  struct Int32Eulers ltp_to_body_euler;
-  INT32_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
-  ltp_to_body_euler.phi -= ANGLE_BFP_OF_REAL(ins_roll_neutral);
-  ltp_to_body_euler.theta -= ANGLE_BFP_OF_REAL(ins_pitch_neutral);
-  stateSetNedToBodyEulers_i(&ltp_to_body_euler);
-#else
   stateSetNedToBodyRMat_i(&ltp_to_body_rmat);
-#endif
 
   struct Int32Rates body_rate;
   /* compute body rates */

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
@@ -49,9 +49,4 @@ struct AhrsIntCmplEuler {
 extern struct AhrsIntCmplEuler ahrs_impl;
 
 
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-extern float ins_roll_neutral;
-extern float ins_pitch_neutral;
-#endif
-
 #endif /* AHRS_INT_CMPL_EULER_H */