Copter: replace set_next_WP with direct call to wp_nav

ArduCopter/ArduCopter.pde

@@ -668,14 +668,10 @@ static struct   Location home;
 static struct   Location current_loc;
 // Next WP is the desired location of the copter - the next waypoint or loiter location
 static struct   Location next_WP;
-// Prev WP is used to get the optimum path from one WP to the next
-static struct   Location prev_WP;
 // Holds the current loaded command from the EEPROM for navigation
 static struct   Location command_nav_queue;
 // Holds the current loaded command from the EEPROM for conditional scripts
 static struct   Location command_cond_queue;
-// Holds the current loaded command from the EEPROM for guided mode
-static struct   Location guided_WP;
 
 
 ////////////////////////////////////////////////////////////////////////////////

ArduCopter/GCS_Mavlink.pde

@@ -1695,14 +1695,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         }
 
         if(packet.current == 2) {                                               //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
-            guided_WP = tell_command;
+            // switch to guided mode
+            set_mode(GUIDED);
 
-            // add home alt if needed
-            if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) {
-                guided_WP.alt += home.alt;
-            }
+            // set wp_nav's destination
+            wp_nav.set_destination(pv_location_to_vector(tell_command));
 
-            set_mode(GUIDED);
+            // set altitude target
+            set_new_altitude(tell_command.alt);
 
             // verify we recevied the command
             mavlink_msg_mission_ack_send(

ArduCopter/commands.pde

@@ -113,63 +113,6 @@ static int32_t get_RTL_alt()
     }
 }
 
-
-//********************************************************************************
-// This function sets the waypoint and modes for Return to Launch
-// It's not currently used
-//********************************************************************************
-
-/*
- *  This function sets the next waypoint command
- *  It precalculates all the necessary stuff.
- */
-
-static void set_next_WP(const struct Location *wp)
-{
-    // copy the current WP into the OldWP slot
-    // ---------------------------------------
-    if (next_WP.lat == 0 || command_nav_index <= 1) {
-        prev_WP = current_loc;
-    }else{
-        if (get_distance_cm(&current_loc, &next_WP) < 500)
-            prev_WP = next_WP;
-        else
-            prev_WP = current_loc;
-    }
-
-    // Load the next_WP slot
-    // ---------------------
-    next_WP.options = wp->options;
-    next_WP.lat = wp->lat;
-    next_WP.lng = wp->lng;
-
-    // Set new target altitude so we can track it for climb_rate
-    // To-Do: remove the restriction on negative altitude targets (after testing)
-    set_new_altitude(max(wp->alt,100));
-
-    // recalculate waypoint distance and bearing
-    wp_distance             = get_distance_cm(&current_loc, &next_WP);
-    wp_bearing              = get_bearing_cd(&prev_WP, &next_WP);
-    original_wp_bearing     = wp_bearing;       // to check if we have missed the WP
-}
-
-// set_next_WP_latlon - update just lat and lon targets for nav controllers
-static void set_next_WP_latlon(uint32_t lat, uint32_t lon)
-{
-    // save current location as previous location
-    prev_WP = current_loc;
-
-    // Load the next_WP slot
-    next_WP.lat = lat;
-    next_WP.lng = lon;
-
-    // recalculate waypoint distance and bearing
-    // To-Do: these functions are too expensive to be called every time we update the next_WP
-    wp_distance             = get_distance_cm(&current_loc, &next_WP);
-    wp_bearing              = get_bearing_cd(&prev_WP, &next_WP);
-    original_wp_bearing     = wp_bearing;       // to check if we have missed the WP
-}
-
 // run this at setup on the ground
 // -------------------------------
 static void init_home()
@@ -194,14 +137,6 @@ static void init_home()
     // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
     scaleLongDown = longitude_scale(&home);
     scaleLongUp   = 1.0f/scaleLongDown;
-
-    // Save prev loc this makes the calcs look better before commands are loaded
-    prev_WP = home;
-
-    // Load home for a default guided_WP
-    // -------------
-    guided_WP = home;
-    guided_WP.alt += g.rtl_altitude;
 }
 
 

ArduCopter/commands_logic.pde

@@ -276,6 +276,10 @@ static void do_nav_wp()
     // Set wp navigation target
     wp_nav.set_destination(pv_location_to_vector(command_nav_queue));
 
+    // initialise original_wp_bearing which is used to check if we have missed the waypoint
+    wp_bearing = wp_nav.get_bearing_to_destination();
+    original_wp_bearing = wp_bearing;
+
     // this is our bitmask to verify we have met all conditions to move on
     wp_verify_byte  = 0;
 
@@ -555,8 +559,7 @@ static bool verify_RTL()
                     // land
                     do_land();
                     // override landing location (do_land defaults to current location)
-                    // To-do: ensure this location override is being sent to inav loiter controller
-                    set_next_WP_latlon(home.lat, home.lng);
+                    wp_nav.set_loiter_target(Vector3f(0,0,0));
                     // update RTL state
                     rtl_state = RTL_STATE_LAND;
                 }else{

ArduCopter/limits.pde

@@ -30,12 +30,13 @@ void set_recovery_home_alt() {
     if ((return_altitude_cm_ahl - (uint32_t) home.alt) < 400) {
         return_altitude_cm_ahl = home.alt + 400;
     }
-    guided_WP.id = 0;
-    guided_WP.p1 = 0;
-    guided_WP.options = 0;
-    guided_WP.lat = home.lat;
-    guided_WP.lng = home.lng;
-    guided_WP.alt = return_altitude_cm_ahl;
+    // To-Do: ensure target is fed into wp_nav controller.  Note this function is currently not called
+    //guided_WP.id = 0;
+    //guided_WP.p1 = 0;
+    //guided_WP.options = 0;
+    //guided_WP.lat = home.lat;
+    //guided_WP.lng = home.lng;
+    //guided_WP.alt = return_altitude_cm_ahl;
 }
 
 static void limits_loop() {

ArduCopter/navigation.pde

@@ -98,11 +98,8 @@ static void run_autopilot()
             verify_commands();
             break;
         case GUIDED:
-            // switch to loiter once we've reached the target location and altitude
-            // To-Do: this incorrectly checks verify_nav_wp even though the nav mode may be NAV_LOITER
-            if(verify_nav_wp()) {
-                set_nav_mode(NAV_LOITER);
-            }
+            // no need to do anything - wp_nav should take care of getting us to the desired location
+            break;
         case RTL:
             verify_RTL();
             break;

ArduCopter/system.pde

@@ -439,7 +439,6 @@ static void set_mode(uint8_t mode)
         set_yaw_mode(LOITER_YAW);
         set_roll_pitch_mode(LOITER_RP);
         set_throttle_mode(LOITER_THR);
-        set_next_WP(&current_loc);
         set_nav_mode(LOITER_NAV);
         break;
 
@@ -449,7 +448,6 @@ static void set_mode(uint8_t mode)
         set_yaw_mode(POSITION_YAW);
         set_roll_pitch_mode(POSITION_RP);
         set_throttle_mode(POSITION_THR);
-        set_next_WP(&current_loc);
         set_nav_mode(POSITION_NAV);
         break;
 
@@ -460,9 +458,6 @@ static void set_mode(uint8_t mode)
         set_roll_pitch_mode(GUIDED_RP);
         set_throttle_mode(GUIDED_THR);
         set_nav_mode(GUIDED_NAV);
-        wp_verify_byte = 0;
-        guided_WP = current_loc;
-        set_next_WP(&guided_WP);
         break;
 
     case LAND:
@@ -491,7 +486,6 @@ static void set_mode(uint8_t mode)
         set_roll_pitch_mode(OF_LOITER_RP);
         set_throttle_mode(OF_LOITER_THR);
         set_nav_mode(OF_LOITER_NAV);
-        set_next_WP(&current_loc);
         break;
 
     // THOR