Added orderCommandList() to be chronological

Fixes #51

src/RemoteTech/FlightComputer/FlightComputer.cs

@@ -128,6 +128,7 @@ public void Enqueue(ICommand cmd, bool ignore_control = false, bool ignore_delay
             if (pos < 0)
             {
                 mCommandQueue.Insert(~pos, cmd);
+                orderCommandList();
             }
         }
 
@@ -312,6 +313,26 @@ public void updatePIDParameters()
             initPIDParameters();
         }
 
+        /// <summary>
+        /// Orders the mCommand queue to be chronological
+        /// </summary>
+        public void orderCommandList()
+        {
+            if (mCommandQueue.Count <= 0) return;
+
+            List<ICommand> backupList = mCommandQueue;
+            // sort the backup queue
+            backupList = backupList.OrderBy(s => (s.TimeStamp + s.ExtraDelay)).ToList();
+            // clear the old queue
+            mCommandQueue.Clear();
+
+            // add the sorted queue
+            foreach(var command in backupList)
+            {
+                mCommandQueue.Add(command);
+            }
+        }
+
         /// <summary>
         /// 
         /// </summary>