Merge branch 'ahrs_sim' into dev

fixes #57

conf/airframes/flixr_discovery.xml

@@ -287,10 +287,10 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
     <define name="AUTO_GROUNDSPEED_IGAIN" value="0."/-->
 
     <!-- Loiter and Dash trimming -->
-    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1000" unit="pprz_t"/>
-    <define name="AUTO_THROTTLE_DASH_TRIM" value="-2000" unit="pprz_t"/>
-    <define name="PITCH_LOITER_TRIM" value="1000" unit="pprz_t"/>
-    <define name="PITCH_DASH_TRIM" value="-2000" unit="pprz_t"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="0" unit="pprz_t"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="0" unit="pprz_t"/>
+    <define name="PITCH_LOITER_TRIM" value="0" unit="pprz_t"/>
+    <define name="PITCH_DASH_TRIM" value="0" unit="pprz_t"/>
 
   </section>
 

conf/autopilot/subsystems/fixedwing/ahrs_float_dcm.makefile

@@ -3,11 +3,10 @@
 # attitude estimation for fixedwings via dcm algorithm
 
 
-$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
-$(TARGET).CFLAGS += -DUSE_AHRS_ALIGNER
-
 ifeq ($(TARGET), ap)
 
+ap.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
+ap.CFLAGS += -DUSE_AHRS_ALIGNER
 ap.CFLAGS += -DUSE_AHRS
 
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
@@ -37,20 +36,14 @@ ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
 
 endif
 
-# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
 
 ifeq ($(TARGET), sim)
 
-sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0
-sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
+sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
+sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
 
-sim.CFLAGS += -DUSE_INFRARED
-sim.srcs += subsystems/sensors/infrared.c
-sim.srcs += subsystems/sensors/infrared_adc.c
-
-sim.srcs += $(SRC_ARCH)/sim_ir.c
-sim.srcs += $(SRC_ARCH)/sim_imu.c
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
 
 endif
 
-jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c

conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile

@@ -17,8 +17,17 @@ AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ap.CFLAGS += $(AHRS_CFLAGS)
 ap.srcs += $(AHRS_SRCS)
 
-sim.CFLAGS += $(AHRS_CFLAGS)
-sim.srcs += $(AHRS_SRCS)
+
+ifeq ($(TARGET), sim)
+
+sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
+sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
+
+endif
+
 
 
 # Extra stuff for fixedwings

conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile

@@ -20,8 +20,17 @@ AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ap.CFLAGS += $(AHRS_CFLAGS)
 ap.srcs += $(AHRS_SRCS)
 
-sim.CFLAGS += $(AHRS_CFLAGS)
-sim.srcs += $(AHRS_SRCS)
+
+
+ifeq ($(TARGET), sim)
+
+sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
+sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
+
+endif
 
 
 # Extra stuff for fixedwings

conf/autopilot/subsystems/fixedwing/autopilot.makefile

@@ -195,6 +195,9 @@ sim.srcs 		+= downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_
 sim.srcs 		+= subsystems/settings.c
 sim.srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c
 
+# hack: always compile some of the sim functions, so ocaml sim does not complain about no-existing functions
+sim.srcs        += $(SRC_ARCH)/sim_ahrs.c $(SRC_ARCH)/sim_ir.c
+
 ######################################################################
 ##
 ## JSBSIM THREAD SPECIFIC

conf/modules/infrared_adc.xml

@@ -35,9 +35,6 @@ endif
 ap.CFLAGS += -DADC_CHANNEL_IR_NB_SAMPLES=$(ADC_IR_NB_SAMPLES)
     </raw>
   </makefile>
-  <makefile target="sim">
-    <file_arch name="sim_ir.c" dir="."/>
-  </makefile>
 </module>
 
 

conf/modules/infrared_i2c.xml

@@ -14,8 +14,5 @@
     <file name="infrared.c" dir="subsystems/sensors"/>
     <file name="infrared_i2c.c" dir="subsystems/sensors"/>
   </makefile>
-  <makefile target="sim">
-    <file_arch name="sim_ir.c" dir="."/>
-  </makefile>
 </module>
 

sw/airborne/ap_downlink.h

@@ -95,7 +95,7 @@
 
 #ifdef USE_INFRARED
 #define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_chan, &mde, &contrast); }
-#elif defined USE_AHRS
+#elif defined USE_IMU && defined USE_AHRS
 #define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; DOWNLINK_SEND_STATE_FILTER_STATUS(_chan, &mde, &val); }
 #else
 #define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) {}
@@ -124,6 +124,7 @@
 #endif
 
 #if defined USE_INFRARED || USE_INFRARED_TELEMETRY
+#include "subsystems/sensors/infrared.h"
 #define PERIODIC_SEND_IR_SENSORS(_chan) DOWNLINK_SEND_IR_SENSORS(_chan, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
 #else
 #define PERIODIC_SEND_IR_SENSORS(_chan) ;

sw/airborne/arch/sim/modules/ins/ins_arduimu.c

@@ -1,19 +1,11 @@
-/** ArduIMU simulation. OCaml binding.
- *  Sim provides IR sensor reading and airspeed
+/** ArduIMU simulation.
+ *  Sim provides attitude.
  */
 
 
-#include <inttypes.h>
 #include "generated/airframe.h"
-
-#include <caml/mlvalues.h>
-
 #include "estimator.h"
 
-// Prevent undefined reference (from estimator.c)
-#include "subsystems/sensors/infrared.h"
-struct Infrared infrared;
-
 // Arduimu empty implementation
 #include "modules/ins/ins_arduimu.h"
 
@@ -26,24 +18,16 @@ float ins_pitch_neutral;
 float pitch_of_throttle_gain;
 float throttle_slew;
 
+// Updates from Ocaml sim
+extern float sim_phi;
+extern float sim_theta;
+
 void ArduIMU_init( void ) {}
-void ArduIMU_periodic( void ) {}
+void ArduIMU_periodic( void ) {
+  // Feed directly the estimator
+  estimator_phi = sim_phi - ins_roll_neutral;
+  estimator_theta = sim_theta - ins_pitch_neutral;
+}
 void ArduIMU_periodicGPS( void ) {}
 void IMU_Daten_verarbeiten( void ) {}
 
-// Updates from Ocaml sim
-float sim_air_speed;
-
-value set_ir_and_airspeed(
-    value roll __attribute__ ((unused)),
-    value front __attribute__ ((unused)),
-    value top __attribute__ ((unused)),
-    value air_speed
-    ) {
-  // Feed directly the estimator
-  estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral;
-  estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral;
-  sim_air_speed = Double_val(air_speed);
-  return Val_unit;
-}
-

sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c

@@ -1,19 +1,11 @@
-/** ArduIMU simulation. OCaml binding.
- *  Sim provides IR sensor reading and airspeed
+/** ArduIMU simulation.
+ *  Sim provides attitude and rates.
  */
 
 
-#include <inttypes.h>
 #include "generated/airframe.h"
-
-#include <caml/mlvalues.h>
-
 #include "estimator.h"
 
-// Prevent undefined reference (from estimator.c)
-#include "subsystems/sensors/infrared.h"
-struct Infrared infrared;
-
 // Arduimu empty implementation
 #include "modules/ins/ins_arduimu_basic.h"
 
@@ -25,24 +17,19 @@ float ins_roll_neutral;
 float ins_pitch_neutral;
 bool_t arduimu_calibrate_neutrals;
 
+// Updates from Ocaml sim
+extern float sim_phi;
+extern float sim_theta;
+extern float sim_p;
+extern float sim_q;
+
 void ArduIMU_init( void ) {}
-void ArduIMU_periodic( void ) {}
+void ArduIMU_periodic( void ) {
+  // Feed directly the estimator
+  estimator_phi = sim_phi - ins_roll_neutral;
+  estimator_theta = sim_theta - ins_pitch_neutral;
+  estimator_p = sim_p;
+  estimator_q = sim_q;
+}
 void ArduIMU_periodicGPS( void ) {}
 void ArduIMU_event( void ) {}
-
-// Updates from Ocaml sim
-float sim_air_speed;
-
-value set_ir_and_airspeed(
-    value roll __attribute__ ((unused)),
-    value front __attribute__ ((unused)),
-    value top __attribute__ ((unused)),
-    value air_speed
-    ) {
-  // Feed directly the estimator
-  estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral;
-  estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral;
-  sim_air_speed = Double_val(air_speed);
-  return Val_unit;
-}
-

sw/airborne/arch/sim/sim_imu.c

@@ -6,7 +6,6 @@
 
 
 #include <inttypes.h>
-#include "subsystems/sensors/infrared.h"
 #include "generated/airframe.h"
 
 #include <caml/mlvalues.h>

sw/airborne/arch/sim/sim_ir.c

@@ -23,11 +23,11 @@ value set_ir_and_airspeed(
     value air_speed
     ) {
   // INFRARED_TELEMETRY : Stupid hack to use with modules
-//#if defined  USE_INFRARED || USE_INFRARED_TELEMETRY
+#if defined  USE_INFRARED || USE_INFRARED_TELEMETRY
   infrared.roll = Int_val(roll);
   infrared.pitch = Int_val(front);
   infrared.top = Int_val(top);
-//#endif
+#endif
   sim_air_speed = Double_val(air_speed);
   return Val_unit;
 }

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -75,12 +75,16 @@
 #include "subsystems/ahrs/ahrs_aligner.h"
 #endif
 
-#if defined USE_IMU && defined USE_AHRS
+#ifdef USE_AHRS
+#ifdef USE_IMU
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );
 volatile uint8_t ahrs_timeout_counter = 0;
-#endif
+#else
+static inline void on_ahrs_event(void);
+#endif // USE_IMU
+#endif // USE_AHRS
 
 #ifdef USE_GPS
 static inline void on_gps_solution( void );
@@ -374,7 +378,6 @@ static void navigation_task( void ) {
 /**There are four @@@@@ boucles @@@@@:
  * - 20 Hz:
  *   - lets use \a reporting_task at 60 Hz
- *   - updates ir with \a ir_update
  *   - updates estimator of ir with \a ahrs_update_infrared
  *   - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop
  *   - sends to \a fbw \a desired_throttle, \a desired_aileron and
@@ -390,24 +393,25 @@ static void navigation_task( void ) {
 static inline void attitude_loop( void ) {
 
 #ifdef USE_GYRO
-      gyro_update();
+  gyro_update();
 #endif
 
 #ifdef USE_INFRARED
-      ahrs_update_infrared();
+  ahrs_update_infrared();
 #endif /* USE_INFRARED */
-      h_ctl_attitude_loop(); /* Set  h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
-      v_ctl_throttle_slew();
-      ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
-      ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
 
-      ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
+  h_ctl_attitude_loop(); /* Set  h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
+  v_ctl_throttle_slew();
+  ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
+  ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
+
+  ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
 
 #if defined MCU_SPI_LINK
-      link_mcu_send();
+  link_mcu_send();
 #elif defined INTER_MCU && defined SINGLE_MCU
-      /**Directly set the flag indicating to FBW that shared buffer is available*/
-      inter_mcu_received_ap = TRUE;
+  /**Directly set the flag indicating to FBW that shared buffer is available*/
+  inter_mcu_received_ap = TRUE;
 #endif
 
 }
@@ -550,10 +554,12 @@ void init_ap( void ) {
 #ifdef USE_IMU
   imu_init();
 #endif
-#ifdef USE_AHRS
+
 #ifdef USE_AHRS_ALIGNER
   ahrs_aligner_init();
 #endif
+
+#ifdef USE_AHRS
   ahrs_init();
 #endif
 
@@ -609,8 +615,12 @@ void init_ap( void ) {
 /*********** EVENT ***********************************************************/
 void event_task_ap( void ) {
 
-#if defined USE_IMU && defined USE_AHRS
+#if defined USE_AHRS
+#ifdef USE_IMU
   ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
+#else
+  AhrsEvent(on_ahrs_event);
+#endif // USE_IMU
 #endif // USE_AHRS
 
 #ifdef USE_GPS
@@ -658,7 +668,8 @@ static inline void on_gps_solution( void ) {
 #endif
 
 
-#if defined USE_IMU && defined USE_AHRS
+#ifdef USE_AHRS
+#ifdef USE_IMU
 static inline void on_accel_event( void ) {
 }
 
@@ -750,5 +761,15 @@ static inline void on_mag_event(void)
   }
 #endif
 }
+
+#else // USE_IMU not defined
+static inline void on_ahrs_event(void)
+{
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+  ahrs_update_fw_estimator();
+#endif
+}
+#endif // USE_IMU
+
 #endif // USE_AHRS
 

sw/airborne/subsystems/ahrs.c

@@ -20,7 +20,6 @@
  */
 
 #include "subsystems/ahrs.h"
-#include "subsystems/imu.h"
 
 struct Ahrs ahrs;
 struct AhrsFloat ahrs_float;

sw/airborne/subsystems/ahrs.h

@@ -64,7 +64,8 @@ struct AhrsFloat {
   struct FloatRates  body_rate;
   struct FloatRates  body_rate_d;
 
-  uint8_t status;
+  // always use status from fixed point ahrs struct for now
+  //uint8_t status;
 };
 
 extern struct Ahrs ahrs;

sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c

@@ -44,7 +44,7 @@ static inline void compute_body_orientation_and_rates(void);
 struct AhrsFloatCmplRmat ahrs_impl;
 
 void ahrs_init(void) {
-  ahrs_float.status = AHRS_UNINIT;
+  ahrs.status = AHRS_UNINIT;
 
   /* Initialises IMU alignement */
   struct FloatEulers body_to_imu_euler =

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -162,7 +162,7 @@ void ahrs_update_fw_estimator( void )
 
 
 void ahrs_init(void) {
-  ahrs_float.status = AHRS_UNINIT;
+  ahrs.status = AHRS_UNINIT;
 
   /*
    * Initialises our IMU alignement variables

sw/airborne/subsystems/ahrs/ahrs_infrared.c

@@ -29,7 +29,7 @@
 
 
 void ahrs_init(void) {
-  ahrs_float.status = AHRS_UNINIT;
+  ahrs.status = AHRS_UNINIT;
 
   /* set ltp_to_body to zero */
   FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);

sw/airborne/subsystems/ahrs/ahrs_infrared.h

@@ -25,10 +25,19 @@
 #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_gps(void);
 extern void ahrs_update_infrared(void);
 
 // TODO copy ahrs to state instead of estimator
 extern void ahrs_update_fw_estimator(void);
 
+#define AhrsEvent(_available_callback) {        \
+  }
+
 #endif /* AHRS_INFRARED_H */

sw/airborne/subsystems/ahrs/ahrs_sim.c

@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) 2008-2011 The Paparazzi Team
+ *
+ * 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.
+ */
+
+
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_sim.h"
+#include "math/pprz_algebra_float.h"
+#include "generated/airframe.h"
+
+#include <inttypes.h>
+#include <caml/mlvalues.h>
+
+extern float sim_phi;
+extern float sim_theta;
+extern float sim_psi;
+extern float sim_p;
+extern float sim_q;
+extern bool_t ahrs_sim_available;
+
+
+void compute_body_orientation_and_rates(void);
+
+void update_ahrs_from_sim(void) {
+  ahrs_float.ltp_to_imu_euler.phi = sim_phi;
+  ahrs_float.ltp_to_imu_euler.theta = sim_theta;
+  ahrs_float.ltp_to_imu_euler.psi = sim_psi;
+
+  ahrs_float.imu_rate.p = sim_p;
+  ahrs_float.imu_rate.q = sim_q;
+
+  /* set quaternion and rotation matrix representations as well */
+  FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
+  FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
+
+  compute_body_orientation_and_rates();
+}
+
+
+void ahrs_init(void) {
+  //ahrs_float.status = AHRS_UNINIT;
+  // set to running for now and only use ahrs.status, not ahrs_float.status
+  ahrs.status = AHRS_RUNNING;
+
+  ahrs_sim_available = FALSE;
+
+  /* set ltp_to_body to zero */
+  FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
+  FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
+  FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
+  FLOAT_RATES_ZERO(ahrs_float.body_rate);
+
+  /* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */
+  QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat);
+  EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler);
+  RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat);
+  RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate);
+}
+
+void ahrs_align(void)
+{
+  /* Currently not really simulated
+   * body and imu have the same frame and always set to true value from sim
+   */
+
+  update_ahrs_from_sim();
+
+  /* Compute initial body orientation */
+  compute_body_orientation_and_rates();
+
+  ahrs.status = AHRS_RUNNING;
+}
+
+
+void ahrs_propagate(void) {
+}
+
+void ahrs_update_accel(void) {
+}
+
+void ahrs_update_mag(void) {
+}
+
+
+/*
+ * Compute body orientation and rates from imu orientation and rates
+ */
+void compute_body_orientation_and_rates(void) {
+
+  /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */
+
+  QUAT_COPY(ahrs_float.ltp_to_body_quat, ahrs_float.ltp_to_imu_quat);
+  EULERS_COPY(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_imu_euler);
+  RMAT_COPY(ahrs_float.ltp_to_body_rmat, ahrs_float.ltp_to_imu_rmat);
+  RATES_COPY(ahrs_float.body_rate, ahrs_float.imu_rate);
+}
+
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// TODO use ahrs result directly
+#include "estimator.h"
+// 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;
+void ahrs_update_fw_estimator(void)
+{
+  // really subtract ins neutrals here?
+  estimator_phi   = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
+  estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
+  estimator_psi   = ahrs_float.ltp_to_body_euler.psi;
+
+  estimator_p = ahrs_float.body_rate.p;
+  estimator_q = ahrs_float.body_rate.q;
+}
+#endif //AHRS_UPDATE_FW_ESTIMATOR

sw/airborne/subsystems/ahrs/ahrs_sim.h

@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#ifndef AHRS_SIM_H
+#define AHRS_SIM_H
+
+#include "subsystems/ahrs.h"
+#include "std.h"
+
+extern bool_t ahrs_sim_available;
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+#include "estimator.h"
+// TODO copy ahrs to state instead of estimator
+void ahrs_update_fw_estimator(void);
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+#endif
+
+extern void update_ahrs_from_sim(void);
+
+#define AhrsEvent(_available_callback) {        \
+    if (ahrs_sim_available) {                   \
+      update_ahrs_from_sim();               \
+      _available_callback();                    \
+      ahrs_sim_available = FALSE;               \
+    }                                           \
+  }
+
+
+
+#endif /* AHRS_SIM_H */

sw/airborne/subsystems/sensors/infrared.h

@@ -92,6 +92,18 @@
 #define IR_CORRECTION_DOWN 1.
 #endif
 
+
+/*
+ * Default neutral values
+ */
+#ifndef IR_ROLL_NEUTRAL_DEFAULT
+#define IR_ROLL_NEUTRAL_DEFAULT 0.0
+#endif
+
+#ifndef IR_PITCH_NEUTRAL_DEFAULT
+#define IR_PITCH_NEUTRAL_DEFAULT 0.0
+#endif
+
 struct Infrared_raw {
   /* the 3 channels of the sensor
    */

sw/simulator/flightModel.ml

@@ -69,6 +69,7 @@ module type SIG =
 let get_xyz state = (state.x, state.y, state.z)
 let get_time state = state.t
 let get_attitude state = (state.phi, state.theta, state.psi)
+let get_pq state = (state.phi_dot, state.theta_dot)
 
 let get_air_speed state = state.air_speed
 let set_air_speed state = fun s -> state.air_speed <- s

sw/simulator/flightModel.mli

@@ -33,6 +33,7 @@ type state
 val get_xyz : state -> meter * meter * meter
 val get_time : state -> float
 val get_attitude : state -> radian * radian * radian
+val get_pq : state -> radian_s * radian_s
 
 val set_air_speed : state -> meter_s -> unit
 val get_air_speed : state -> meter_s

sw/simulator/hitl.ml

@@ -97,6 +97,9 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     with
       exc -> prerr_endline (Printexc.to_string exc)
 
+  let attitude_and_rates = fun phi theta psi p q ->
+    prerr_endline "HITL attitude sim not implemented..."
+
   let sep_reg = Str.regexp Pprz.separator
   let read_commands = fun commands _sender values ->
     let s = Pprz.string_assoc "values" values in

sw/simulator/sim.ml

@@ -43,6 +43,7 @@ let georef_of_xml = fun xml ->
 let ir_period = 1./.20.
 let fm_period = 1./.25.
 let fg_period = 1./.25.
+let ahrs_period = 1./.20.
 
 let gensym = let n = ref 0 in fun p -> incr n; p ^ string_of_int !n
 let cb_register = fun closure ->
@@ -64,6 +65,9 @@ module type AIRCRAFT =
     val infrared_and_airspeed : float -> float -> float -> float -> unit
 	(** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
 
+    val attitude_and_rates : float -> float -> float -> float -> float ->unit
+    (** [ahrs phi theta psi p q] Called on timer *)
+
     val gps : Gps.state -> unit
 	(** [gps state] Called on timer *)
   end
@@ -217,7 +221,12 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       let s = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
       s.Gps.availability <- not (!gps_availability = 0);
       last_gps_state := Some s;
-      Aircraft.gps s in
+      Aircraft.gps s
+
+    and ahrs_task = fun () ->
+      let (phi, theta, psi) = FlightModel.get_attitude !state
+      and p, q = FlightModel.get_pq !state in
+      Aircraft.attitude_and_rates phi theta psi p q in
 
     (** Sending to Flight Gear *)
     let fg_task = fun socket buffer () ->
@@ -257,6 +266,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       Stdlib.timer ~scale:time_scale fm_period fm_task;
       Stdlib.timer ~scale:time_scale ir_period ir_task;
       Stdlib.timer ~scale:time_scale gps_period gps_task;
+      Stdlib.timer ~scale:time_scale ahrs_period ahrs_task;
 
       (** Connection to Flight Gear client *)
       if !fg_client <> "" then

sw/simulator/sim.mli

@@ -10,6 +10,7 @@ module type AIRCRAFT =
     val boot : Stdlib.value -> unit
     val commands : Stdlib.pprz_t array -> unit
     val infrared_and_airspeed : float -> float -> float -> float -> unit
+    val attitude_and_rates : float -> float -> float -> float -> float -> unit
     val gps : Gps.state -> unit
   end
 

sw/simulator/sitl.ml

@@ -194,6 +194,10 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     (** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
     set_ir_and_airspeed (truncate ir_left) (truncate ir_front) (truncate ir_top) air_speed
 
+  external provide_attitude_and_rates : float -> float -> float -> float -> float -> unit = "provide_attitude_and_rates"
+  let attitude_and_rates = fun phi theta psi p q ->
+    provide_attitude_and_rates phi theta psi p q
+
   external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> float -> float -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos"
   let gps = fun gps ->
     let utm = utm_of WGS84 gps.Gps.wgs84 in