From 74dca6da2218e0aeafddbc2ef0c085d0383d9019 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 24 Apr 2013 20:59:49 +0900
Subject: [PATCH] Copter: move update_commands to run_autopilot fn

update_commands was being run in the medium_loop counter but it is
easier to understand the flow of the code if it is consolidated along
with other autopilot calls.
---
 ArduCopter/ArduCopter.pde       |  8 --------
 ArduCopter/commands_process.pde | 15 ++++++---------
 ArduCopter/navigation.pde       |  5 ++++-
 3 files changed, 10 insertions(+), 18 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 7321a77b61..a5d13d4101 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1094,14 +1094,6 @@ static void medium_loop()
     case 3:
         medium_loopCounter++;
 
-        // perform next command
-        // --------------------
-        if(control_mode == AUTO) {
-            if(ap.home_is_set && (g.command_total > 1)) {
-                update_commands();
-            }
-        }
-
         if(motors.armed()) {
             if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {
                 Log_Write_Attitude();
diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde
index 51f94f7a13..550546b233 100644
--- a/ArduCopter/commands_process.pde
+++ b/ArduCopter/commands_process.pde
@@ -142,26 +142,24 @@ static void execute_nav_command(void)
 }
 
 // verify_commands - high level function to check if navigation and conditional commands have completed
-// called after GPS navigation update - not constantly
 static void verify_commands(void)
 {
+    // check if navigation command completed
     if(verify_must()) {
-        //cliSerial->printf("verified must cmd %d\n" , command_nav_index);
+        // clear navigation command queue so next command can be loaded
         command_nav_queue.id    = NO_COMMAND;
 
         // store our most recent executed nav command
-        prev_nav_index                  = command_nav_index;
+        prev_nav_index          = command_nav_index;
 
         // Wipe existing conditionals
-        command_cond_index              = NO_COMMAND;
+        command_cond_index      = NO_COMMAND;
         command_cond_queue.id   = NO_COMMAND;
-
-    }else{
-        //cliSerial->printf("verified must false %d\n" , command_nav_index);
     }
 
+    // check if conditional command completed
     if(verify_may()) {
-        //cliSerial->printf("verified may cmd %d\n" , command_cond_index);
+        // clear conditional command queue so next command can be loaded
         command_cond_queue.id = NO_COMMAND;
     }
 }
@@ -195,7 +193,6 @@ static void exit_mission()
             set_mode(LAND);
         }else{
             set_mode(LOITER);
-            wp_nav.set_desired_alt(g.rtl_alt_final);
         }
     }
 
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 3cc4c43b3e..77484872e0 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -85,7 +85,10 @@ static void run_autopilot()
 {
     switch( control_mode ) {
         case AUTO:
-            // majority of command logic is in commands_logic.pde
+            // load the next command if the command queues are empty
+            update_commands();
+
+            // process the active navigation and conditional commands
             verify_commands();
             break;
         case GUIDED: