AP_Mission: added semaphore for thread access

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;

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;
 };