From e1ff9a641b9a1a80681792aefab23f759253bf10 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 5 Mar 2012 21:00:22 +1100
Subject: [PATCH] SITL: prevent nested timer interrupts

---
 libraries/Desktop/support/sitl.cpp     | 7 +++++--
 libraries/Desktop/support/sitl_adc.cpp | 2 --
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp
index 67661d5d29..0671be6caf 100644
--- a/libraries/Desktop/support/sitl.cpp
+++ b/libraries/Desktop/support/sitl.cpp
@@ -146,7 +146,6 @@ static void sitl_fdm_input(void)
 	case 16: {
 		// a packet giving the receiver PWM inputs
 		uint8_t i;
-		cli();
 		for (i=0; i<8; i++) {
 			// setup the ICR4 register for the RC channel
 			// inputs
@@ -154,7 +153,6 @@ static void sitl_fdm_input(void)
 				ICR4.set(i, d.pwm_pkt.pwm[i]);
 			}
 		}
-		sei();
 		break;
 	}
 	}
@@ -221,6 +219,8 @@ static void timer_handler(int signum)
 {
 	static uint32_t last_update_count;
 
+	cli();
+
 	/* make sure we die if our parent dies */
 	if (kill(parent_pid, 0) != 0) {
 		exit(1);
@@ -242,10 +242,12 @@ static void timer_handler(int signum)
 
 	if (update_count == 0) {
 		sitl_update_gps(0, 0, 0, 0, 0, false);
+		sei();
 		return;
 	}
 
 	if (update_count == last_update_count) {
+		sei();
 		return;
 	}
 	last_update_count = update_count;
@@ -259,6 +261,7 @@ static void timer_handler(int signum)
 			sim_state.airspeed);
 	sitl_update_barometer(sim_state.altitude);
 	sitl_update_compass(sim_state.heading, sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading);
+	sei();
 }
 
 
diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp
index 0b25f50644..b3339d9143 100644
--- a/libraries/Desktop/support/sitl_adc.cpp
+++ b/libraries/Desktop/support/sitl_adc.cpp
@@ -89,7 +89,6 @@ void sitl_update_adc(float roll, 	float pitch, 	float yaw,		// Relative to earth
 	adc[5] =  (zAccel / (_accel_scale * _sensor_signs[5])) + accel_offset;
 
 	/* tell the UDR2 register emulation what values to give to the driver */
-	cli();
 	for (uint8_t i=0; i<6; i++) {
 		UDR2.set(sensor_map[i], adc[i]);
 	}
@@ -98,7 +97,6 @@ void sitl_update_adc(float roll, 	float pitch, 	float yaw,		// Relative to earth
 
 	/* FIX: rubbish value for temperature for now */
 	UDR2.set(3, 2000);
-	sei();
 
 	runInterrupt(6);
 }