From 0289ad03dfbe6bd49f492061c2737b505f1c8eca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 20 Aug 2018 11:37:54 +1000
Subject: [PATCH] AP_Mission: added semaphore for thread access

---
 libraries/AP_Mission/AP_Mission.cpp |  6 ++++++
 libraries/AP_Mission/AP_Mission.h   | 10 ++++++++++
 2 files changed, 16 insertions(+)

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index 0bc405a0d5..9b9453b1b9 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -37,6 +37,8 @@ extern const AP_HAL::HAL& hal;
 // storage object
 StorageAccess AP_Mission::_storage(StorageManager::StorageMission);
 
+HAL_Semaphore_Recursive AP_Mission::_rsem;
+
 ///
 /// public mission methods
 ///
@@ -463,6 +465,8 @@ bool AP_Mission::set_current_cmd(uint16_t index)
 ///     true is return if successful
 bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const
 {
+    WITH_SEMAPHORE(_rsem);
+    
     // exit immediately if index is beyond last command but we always let cmd #0 (i.e. home) be read
     if (index >= (unsigned)_cmd_total && index != 0) {
         return false;
@@ -504,6 +508,8 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
 ///     true is returned if successful
 bool AP_Mission::write_cmd_to_storage(uint16_t index, Mission_Command& cmd)
 {
+    WITH_SEMAPHORE(_rsem);
+    
     // range check cmd's index
     if (index >= num_commands_max()) {
         return false;
diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
index 5f7f8634ac..b2073d8996 100644
--- a/libraries/AP_Mission/AP_Mission.h
+++ b/libraries/AP_Mission/AP_Mission.h
@@ -460,6 +460,12 @@ public:
     // available.
     bool jump_to_landing_sequence(void);
 
+    // get a reference to the AP_Mission semaphore, allowing an external caller to lock the
+    // storage while working with multiple waypoints
+    HAL_Semaphore_Recursive &get_semaphore(void) {
+        return _rsem;
+    }
+
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -551,4 +557,8 @@ private:
 
     // last time that mission changed
     uint32_t _last_change_time_ms;
+
+    // multi-thread support. This is static so it can be used from
+    // const functions
+    static HAL_Semaphore_Recursive _rsem;
 };