From c257cea375922febc03227ad7b2793f0288ad887 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Tue, 11 Feb 2020 16:10:45 -0700
Subject: [PATCH] AP_Scheduler: Add a lock that is held during all normal
 operations

---
 libraries/AP_Scheduler/AP_Scheduler.cpp | 5 +++++
 libraries/AP_Scheduler/AP_Scheduler.h   | 6 ++++++
 2 files changed, 11 insertions(+)

diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp
index 0620897948..0ef9810866 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.cpp
+++ b/libraries/AP_Scheduler/AP_Scheduler.cpp
@@ -87,6 +87,9 @@ AP_Scheduler *AP_Scheduler::get_singleton()
 // initialise the scheduler
 void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit)
 {
+    // grab the semaphore before we start anything
+    _rsem.take_blocking();
+
     // only allow 50 to 2000 Hz
     if (_loop_rate_hz < 50) {
         _loop_rate_hz.set(50);
@@ -267,7 +270,9 @@ void AP_Scheduler::loop()
 {
     // wait for an INS sample
     hal.util->persistent_data.scheduler_task = -3;
+    _rsem.give();
     AP::ins().wait_for_sample();
+    _rsem.take_blocking();
     hal.util->persistent_data.scheduler_task = -1;
 
     const uint32_t sample_time_us = AP_HAL::micros();
diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h
index e902a09373..c2d6c4301a 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.h
+++ b/libraries/AP_Scheduler/AP_Scheduler.h
@@ -144,6 +144,8 @@ public:
         return extra_loop_us;
     }
 
+    HAL_Semaphore &get_semaphore(void) { return _rsem; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
     // loop performance monitoring:
@@ -223,6 +225,10 @@ private:
     // extra time available for each loop - used to dynamically adjust
     // the loop rate in case we are well over budget
     uint32_t extra_loop_us;
+
+
+    // semaphore that is held while not waiting for ins samples
+    HAL_Semaphore _rsem;
 };
 
 namespace AP {