added ahrs_infrared subsystem makefile and updated the code

conf/autopilot/subsystems/fixedwing/ahrs_infrared.makefile

@@ -0,0 +1,54 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# attitude estimation for fixedwings using infrared sensors
+
+#
+# default values for tiny and twog are:
+# ADC_IR1    = ADC_1
+# ADC_IR2    = ADC_2
+# ADC_IR_TOP = ADC_0
+# ADC_IR_NB_SAMPLES = 16
+#
+# to change just redefine these before including this file
+#
+
+#
+# LPC only has one ADC
+#
+ifeq ($(ARCH), lpc21)
+ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1) -DUSE_$(ADC_IR1)
+ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2) -DUSE_$(ADC_IR2)
+ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP) -DUSE_$(ADC_IR_TOP)
+endif
+
+#
+# On STM32 let's hardwire infrared sensors to AD1 for now
+#
+ifeq ($(ARCH), stm32)
+ap.CFLAGS += -DUSE_AD1
+ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1_CHAN) -DUSE_AD1_$(ADC_IR1)
+ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2_CHAN) -DUSE_AD1_$(ADC_IR2)
+ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP_CHAN) -DUSE_AD1_$(ADC_IR_TOP)
+endif
+
+ap.CFLAGS += -DADC_CHANNEL_IR_NB_SAMPLES=$(ADC_IR_NB_SAMPLES)
+
+$(TARGET).CFLAGS += -DUSE_INFRARED
+$(TARGET).srcs += subsystems/sensors/infrared.c
+$(TARGET).srcs += subsystems/sensors/infrared_adc.c
+
+
+
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_infrared.h\"
+$(TARGET).CFLAGS += -DUSE_AHRS
+
+$(TARGET).srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
+$(TARGET).srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_infrared.c
+
+
+ifeq ($(TARGET), sim)
+
+sim.srcs += $(SRC_ARCH)/sim_ir.c
+
+endif
+

sw/airborne/subsystems/ahrs/ahrs_infrared.c

@@ -20,7 +20,13 @@
  */
 
 #include "subsystems/ahrs/ahrs_infrared.h"
+
 #include "subsystems/sensors/infrared.h"
+#include "subsystems/gps.h"
+
+#include "estimator.h"
+
+
 
 void ahrs_init(void) {
   ahrs_float.status = AHRS_UNINIT;
@@ -54,6 +60,22 @@ void ahrs_update_accel(void) {
 void ahrs_update_mag(void) {
 }
 
+void ahrs_update_gps(void) {
+
+  float hspeed_mod_f = gps.gspeed / 100.;
+  float course_f = gps.course / 1e7;
+
+  // Heading estimator from wind-information, usually computed with -DWIND_INFO
+  // wind_north and wind_east initialized to 0, so still correct if not updated
+  float w_vn = cosf(course_f) * hspeed_mod_f - wind_north;
+  float w_ve = sinf(course_f) * hspeed_mod_f - wind_east;
+  ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn);
+  if (ahrs_float.ltp_to_body_euler.psi < 0.)
+    ahrs_float.ltp_to_body_euler.psi += 2 * M_PI;
+
+  ahrs_update_fw_estimator();
+}
+
 void ahrs_update_infrared(void) {
   ahrs_float.ltp_to_body_euler.phi  = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
 
@@ -73,4 +95,21 @@ void ahrs_update_infrared(void) {
     ahrs_float.ltp_to_body_euler.theta *= infrared.correction_up;
   else
     ahrs_float.ltp_to_body_euler.theta *= infrared.correction_down;
+
+  ahrs_update_fw_estimator();
+}
+
+
+// TODO use ahrs result directly
+void ahrs_update_fw_estimator(void)
+{
+  // export results to estimator
+
+  estimator_phi   = ahrs_float.ltp_to_body_euler.phi;
+  estimator_theta = ahrs_float.ltp_to_body_euler.theta;
+  estimator_psi   = ahrs_float.ltp_to_body_euler.psi;
+
+  estimator_p = ahrs_float.body_rate.p;
+  estimator_q = ahrs_float.body_rate.q;
+
 }

sw/airborne/subsystems/ahrs/ahrs_infrared.h

@@ -25,5 +25,10 @@
 #include "subsystems/ahrs.h"
 #include "std.h"
 
+extern void ahrs_update_gps(void);
+extern void ahrs_update_infrared(void);
+
+// TODO copy ahrs to state instead of estimator
+extern void ahrs_update_fw_estimator(void);
 
 #endif /* AHRS_INFRARED_H */