From 0534406f6d3a7b3002ee7bc2bebe563ebcc24a4f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 19 Nov 2012 08:26:53 +1100
Subject: [PATCH] SITL: added SREG declaration
---
libraries/Desktop/include/avr/io.h | 4 ++++
libraries/Desktop/support/Arduino.cpp | 11 +++--------
libraries/Desktop/support/sitl.cpp | 7 +------
3 files changed, 8 insertions(+), 14 deletions(-)
diff --git a/libraries/Desktop/include/avr/io.h b/libraries/Desktop/include/avr/io.h
index 695adec7e3..fa58db23a1 100644
--- a/libraries/Desktop/include/avr/io.h
+++ b/libraries/Desktop/include/avr/io.h
@@ -15,6 +15,10 @@ extern "C" volatile uint8_t __iomem[1024];
#define _SFR_IO8(addr) __iomem[addr]
+extern "C" volatile uint8_t SREG;
+
+#define _interrupts_are_blocked() ((SREG&0x80)==0)
+
#define __ATmegaxx0__
#include "iom2560.h"
diff --git a/libraries/Desktop/support/Arduino.cpp b/libraries/Desktop/support/Arduino.cpp
index a0c6c8e017..8671e7860d 100644
--- a/libraries/Desktop/support/Arduino.cpp
+++ b/libraries/Desktop/support/Arduino.cpp
@@ -11,6 +11,7 @@
extern "C" {
volatile uint8_t __iomem[1024];
+volatile uint8_t SREG = 0x80;
unsigned __brkval = 0x2000;
unsigned __bss_end = 0x1000;
@@ -18,19 +19,13 @@ unsigned __bss_end = 0x1000;
// disable interrupts
void cli(void)
{
- sigset_t set;
- sigemptyset(&set);
- sigaddset(&set, SIGALRM);
- sigprocmask(SIG_BLOCK,&set,NULL);
+ SREG &= ~0x80;
}
// enable interrupts
void sei(void)
{
- sigset_t set;
- sigemptyset(&set);
- sigaddset(&set, SIGALRM);
- sigprocmask(SIG_UNBLOCK,&set,NULL);
+ SREG |= 0x80;
}
void pinMode(uint8_t pin, uint8_t mode)
diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp
index 80900bfe12..363f7840ad 100644
--- a/libraries/Desktop/support/sitl.cpp
+++ b/libraries/Desktop/support/sitl.cpp
@@ -228,12 +228,10 @@ static void sitl_simulator_output(void)
static void timer_handler(int signum)
{
static uint32_t last_update_count;
- static bool running;
- if (running) {
+ if (_interrupts_are_blocked()) {
return;
}
- running = true;
cli();
#ifndef __CYGWIN__
@@ -271,13 +269,11 @@ static void timer_handler(int signum)
if (update_count == 0) {
sitl_update_gps(0, 0, 0, 0, 0, false);
sei();
- running = false;
return;
}
if (update_count == last_update_count) {
sei();
- running = false;
return;
}
last_update_count = update_count;
@@ -297,7 +293,6 @@ static void timer_handler(int signum)
ADCSRA &= ~_BV(ADSC);
sei();
- running = false;
}