diff --git a/conf/airframes/microjet5_tp_auto.xml b/conf/airframes/microjet5_tp_auto.xml
index 782b6b23fa..d848720686 100644
--- a/conf/airframes/microjet5_tp_auto.xml
+++ b/conf/airframes/microjet5_tp_auto.xml
@@ -14,8 +14,6 @@
-
-
@@ -47,7 +45,6 @@
-
@@ -57,15 +54,13 @@
@@ -80,10 +75,7 @@
@@ -110,12 +102,11 @@
-
-
-
+
+
-
+
@@ -136,7 +127,7 @@
-
+
@@ -156,17 +147,6 @@
-
-
@@ -242,9 +222,10 @@ ap.srcs += gyro.c
# Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DCONFIG=\"tiny.h\" -DLOITER_TRIM -DMOBILE_CAM -DCAM
-sim.srcs += nav_line.c traffic_info.c cam.c
-
+sim.CFLAGS += -DCONFIG=\"tiny.h\"
+sim.srcs += nav_survey_rectangle.c traffic_info.c nav_line.c
+sim.CFLAGS += -DGYRO -DADXRS150
+sim.srcs += gyro.c
diff --git a/conf/settings/tuning_tp_auto.xml b/conf/settings/tuning_tp_auto.xml
index 27493478bf..8f86a5f4c6 100644
--- a/conf/settings/tuning_tp_auto.xml
+++ b/conf/settings/tuning_tp_auto.xml
@@ -6,78 +6,73 @@
-
-
-
-
+
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
-
+
-
-
-
-
-
+
+
+
+
+
+
+
-
+
+
+
+
+
diff --git a/sw/airborne/fw_h_ctl.c b/sw/airborne/fw_h_ctl.c
index 20323c9223..39c51d1068 100644
--- a/sw/airborne/fw_h_ctl.c
+++ b/sw/airborne/fw_h_ctl.c
@@ -248,6 +248,11 @@ void h_ctl_attitude_loop ( void ) {
#ifdef H_CTL_ROLL_ATTITUDE_GAIN
inline static void h_ctl_roll_loop( void ) {
float err = estimator_phi - h_ctl_roll_setpoint;
+#ifdef SITL
+ static float last_err = 0;
+ estimator_p = (err - last_err)/(1/60.);
+ last_err = err;
+#endif
float cmd = - h_ctl_roll_attitude_gain * err
- h_ctl_roll_rate_gain * estimator_p
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
diff --git a/sw/airborne/fw_v_ctl.h b/sw/airborne/fw_v_ctl.h
index a37daa6567..20c8574a82 100644
--- a/sw/airborne/fw_v_ctl.h
+++ b/sw/airborne/fw_v_ctl.h
@@ -97,4 +97,9 @@ extern void v_ctl_throttle_slew( void );
Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); \
}
+#define fw_v_ctl_SetAutoThrottleIgain(_v) { \
+ v_ctl_auto_throttle_igain = _v; \
+ v_ctl_auto_throttle_sum_err = 0; \
+ }
+
#endif /* FW_V_CTL_H */
diff --git a/sw/airborne/gyro.c b/sw/airborne/gyro.c
index fccf2c3b75..a5575b63b7 100644
--- a/sw/airborne/gyro.c
+++ b/sw/airborne/gyro.c
@@ -50,6 +50,7 @@ static struct adc_buf buf_pitch;
#endif
void gyro_init( void) {
+#ifndef SITL
adc_buf_channel(ADC_CHANNEL_GYRO_ROLL, &buf_roll, ADC_CHANNEL_GYRO_NB_SAMPLES);
#if defined ADC_CHANNEL_GYRO_TEMP
adc_buf_channel(ADC_CHANNEL_GYRO_TEMP, &buf_temp, ADC_CHANNEL_GYRO_NB_SAMPLES);
@@ -57,11 +58,13 @@ void gyro_init( void) {
#if defined ADC_CHANNEL_GYRO_PITCH
adc_buf_channel(ADC_CHANNEL_GYRO_PITCH, &buf_pitch, ADC_CHANNEL_GYRO_NB_SAMPLES);
#endif
+#endif /* SITL */
}
void gyro_update( void ) {
+#ifndef SITL
float pitch_rate = 0.;
roll_rate_adc = (buf_roll.sum/buf_roll.av_nb_sample) - GYRO_ADC_ROLL_NEUTRAL;
#if defined ADC_CHANNEL_GYRO_TEMP
@@ -74,4 +77,5 @@ void gyro_update( void ) {
#endif
float roll_rate = GYRO_ROLL_DIRECTION * RadiansOfADC(roll_rate_adc, GYRO_ROLL_SCALE);
EstimatorSetRate(roll_rate, pitch_rate);
+#endif /* SITL */
}
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index 542c2ba752..52ecbcf68e 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -191,7 +191,7 @@ module Make(A:Data.MISSION) = struct
and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in
state.x <- state.x +.dx ;
state.y <- state.y +. dy;
- let gamma = (state.thrust -. drag) /. weight +. state.theta in
+ let gamma = (state.thrust -. drag) /. weight +. state.theta -. 0.1 *. abs_float (sin state.phi)) in
let dz = sin gamma *. state.air_speed *. dt in
state.z <- state.z +. dz;