diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4bf408cfbc..981f2d5ce7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/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; //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index f11ccb7578..538757c2c7 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1695,15 +1695,15 @@ 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; - - // add home alt if needed - if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) { - guided_WP.alt += home.alt; - } - + // switch to guided mode set_mode(GUIDED); + // set wp_nav's destination + wp_nav.set_destination(pv_location_to_vector(tell_command)); + + // set altitude target + set_new_altitude(tell_command.alt); + // verify we recevied the command mavlink_msg_mission_ack_send( chan, diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index ef17cc652d..c5635fe84f 100644 --- a/ArduCopter/commands.pde +++ b/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(¤t_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(¤t_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(¤t_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; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 40f162f086..510cfb1997 100644 --- a/ArduCopter/commands_logic.pde +++ b/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{ diff --git a/ArduCopter/limits.pde b/ArduCopter/limits.pde index 54691ab0ba..6f3004f5b2 100644 --- a/ArduCopter/limits.pde +++ b/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() { diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 568ea40ff9..3e0255e8ad 100644 --- a/ArduCopter/navigation.pde +++ b/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; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index f53cd17e2b..aeedc36373 100644 --- a/ArduCopter/system.pde +++ b/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(¤t_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(¤t_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(¤t_loc); break; // THOR