From 5800c2a2c85e064ea4c041cd0fa11ce433c8a5a0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 28 Oct 2013 16:10:51 +1100
Subject: [PATCH] HAL_PX4: prevent excessive writes on startup from blocking

this could cause copter on PX4 to hang on startup
---
 libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 2 ++
 libraries/AP_HAL_PX4/Scheduler.cpp     | 6 +++---
 libraries/AP_HAL_PX4/Scheduler.h       | 4 +++-
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index 760a3ed071..cc56834724 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -120,6 +120,8 @@ static int main_loop(int argc, char **argv)
      */
     set_priority(APM_STARTUP_PRIORITY);
 
+    schedulerInstance.hal_initialized();
+
     setup();
     hal.scheduler->system_initialized();
 
diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index 5ad1c4d655..1e35bf6c66 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -231,7 +231,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
 
 void *PX4Scheduler::_timer_thread(void)
 {
-    while (system_initializing()) {
+    while (!_hal_initialized) {
         poll(NULL, 0, 1);        
     }
     while (!_px4_thread_should_exit) {
@@ -272,7 +272,7 @@ void PX4Scheduler::_run_io(void)
 
 void *PX4Scheduler::_uart_thread(void)
 {
-    while (system_initializing()) {
+    while (!_hal_initialized) {
         poll(NULL, 0, 1);        
     }
     while (!_px4_thread_should_exit) {
@@ -288,7 +288,7 @@ void *PX4Scheduler::_uart_thread(void)
 
 void *PX4Scheduler::_io_thread(void)
 {
-    while (system_initializing()) {
+    while (!_hal_initialized) {
         poll(NULL, 0, 1);        
     }
     while (!_px4_thread_should_exit) {
diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h
index 5ad27a0327..b88b997dd1 100644
--- a/libraries/AP_HAL_PX4/Scheduler.h
+++ b/libraries/AP_HAL_PX4/Scheduler.h
@@ -42,9 +42,11 @@ public:
     bool     in_timerprocess();
     bool     system_initializing();
     void     system_initialized();
-
+    void     hal_initialized() { _hal_initialized = true; }
+    
 private:
     bool _initialized;
+    volatile bool _hal_initialized;
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
     AP_HAL::Proc _failsafe;