diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 5e98dfc1a2d7..75c8c89a2925 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -20410,6 +20410,7 @@ def tests2a(self): self.MAV_CMD_NAV_TAKEOFF_no_location, self.DualTuningChannels, self.AutoRTL, + self.AutoRTLReturnPathTrack, self.GripperReleaseOnThrustLoss, self.ScriptingAHRSSource, self.CompassLearnCopyFromSIM, @@ -21878,10 +21879,10 @@ def AutoRTL(self): self.change_mode('GUIDED') self.fly_guided_move_to(return_path_test, timeout=100) - # check the mission is joined further along + # check the mission is joined further along, part way along the + # leg from waypoint 4 so waypoint 4 may be current briefly self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) - self.drain_mav() - self.assert_current_waypoint(5) + self.wait_current_waypoint(5, timeout=30) # fly over home home = self.home_position_as_location() @@ -21897,6 +21898,181 @@ def AutoRTL(self): # Done self.land_and_disarm() + def AutoRTLReturnPathTrack(self): + '''Test Auto RTL rejoins a return path at the closest point and then follows the track''' + alt = 50 + self.context_push() + self.context_set_message_rate_hz('LOCAL_POSITION_NED', 10) + + # pymavlink does not understand the new return path command yet, at some point it will + cmd_return_path_start = 188 # mavutil.mavlink.MAV_CMD_DO_RETURN_PATH_START + + def fly_return_path_from(north, east, track_north_max=None, no_backtrack=False, min_alt=None, edit_mission=None): + # the return path runs down the north axis towards home, so any + # east offset south of track_north_max is a cross track error + start_loc = self.offset_location_ne(self.home_position_as_location(), north, east) + start_loc.set_alt_m(alt, AltFrame.ABOVE_HOME) + self.change_mode('GUIDED') + self.fly_guided_move_to(start_loc, timeout=240) + + self.change_mode('AUTO_RTL') + max_cross_track = 0 + max_north = north + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 300: + raise NotAchievedException("Did not fly the return path") + if edit_mission is not None and self.get_sim_time_cached() - tstart > 2: + edit_mission() + edit_mission = None + m = self.assert_receive_message('LOCAL_POSITION_NED') + if m.x < 100: + break + max_north = max(max_north, m.x) + if min_alt is not None and -m.z < min_alt: + raise NotAchievedException(f"Descended to {-m.z:.1f}m before the end of the leg") + if track_north_max is not None and m.x < track_north_max: + max_cross_track = max(max_cross_track, abs(m.y)) + if no_backtrack and max_north > north + 20: + raise NotAchievedException(f"Flew back up the return path to {max_north:.0f}m north") + if track_north_max is not None: + self.progress(f"maximum cross track error {max_cross_track:.1f}m") + if max_cross_track > 10: + raise NotAchievedException(f"Did not follow the return path track (cross track {max_cross_track:.1f}m)") + + self.takeoff(alt, mode='GUIDED') + + # the closest point on the mission is part way along the leg + # from waypoint 3 to waypoint 4, not either waypoint. Nothing + # is placed exactly at home as a zero offset uploads as a zero + # latitude and longitude, which means the current location + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), # 3 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 50, 0, alt), # 4 + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 5 + (mavutil.mavlink.MAV_CMD_NAV_LAND, 50, 0, 0), # 6 + ]) + fly_return_path_from(600, 200, 450) + self.wait_current_waypoint(4) + + # a located command that is not navigated to, such as an ROI, + # is not part of the track. Leave AUTO so the upload does not + # restart the running mission + self.change_mode('GUIDED') + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), # 3 + (mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, 400, 400, alt), # 4 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 50, 0, alt), # 5 + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 6 + (mavutil.mavlink.MAV_CMD_NAV_LAND, 50, 0, 0), # 7 + ]) + fly_return_path_from(400, 300, 350) + self.wait_current_waypoint(5) + + # a repeated waypoint starts the next leg, so the vehicle does not + # fly back up the path to it after the rejoin point + self.change_mode('GUIDED') + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), # 3 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), # 4 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 50, 0, alt), # 5 + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 6 + (mavutil.mavlink.MAV_CMD_NAV_LAND, 50, 0, 0), # 7 + ]) + fly_return_path_from(400, 200, 350, no_backtrack=True) + self.wait_current_waypoint(5) + + # moving the leg start during the rejoin continues to the leg end + # rather than flying back up the path to the moved leg start + def return_path_mission(leg_start_north): + return [ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, leg_start_north, 0, alt), # 3 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 50, 0, alt), # 4 + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 5 + (mavutil.mavlink.MAV_CMD_NAV_LAND, 50, 0, 0), # 6 + ] + self.change_mode('GUIDED') + self.upload_simple_relhome_mission(return_path_mission(800)) + fly_return_path_from(600, 200, no_backtrack=True, + edit_mission=lambda: self.upload_simple_relhome_mission(return_path_mission(850))) + self.wait_current_waypoint(4) + + # leaving AUTO during the rejoin and flying past the rejoin point + # continues to the leg end rather than back to the rejoin point + start_loc = self.offset_location_ne(self.home_position_as_location(), 300, 150) + start_loc.set_alt_m(alt, AltFrame.ABOVE_HOME) + past_loc = self.offset_location_ne(self.home_position_as_location(), 200, 0) + past_loc.set_alt_m(alt, AltFrame.ABOVE_HOME) + self.change_mode('GUIDED') + self.upload_simple_relhome_mission(return_path_mission(800)) + self.fly_guided_move_to(start_loc, timeout=120) + self.change_mode('AUTO_RTL') + self.wait_current_waypoint(3) + self.change_mode('GUIDED') + self.fly_guided_move_to(past_loc, timeout=120) + self.change_mode('AUTO') + self.wait_current_waypoint(4, timeout=5) + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < 3: + m = self.assert_receive_message('LOCAL_POSITION_NED') + if m.x > 210: + raise NotAchievedException(f"Flew back to the rejoin point, {m.x:.0f}m north") + + # removing the leg end in AUTO during the rejoin keeps the return path, + # and leaving and re-entering AUTO then restarts the new mission + start_loc = self.offset_location_ne(self.home_position_as_location(), 150, 150) + start_loc.set_alt_m(alt, AltFrame.ABOVE_HOME) + self.change_mode('GUIDED') + self.upload_simple_relhome_mission(return_path_mission(800)) + self.fly_guided_move_to(start_loc, timeout=120) + self.change_mode('AUTO_RTL') + self.wait_current_waypoint(3) + self.upload_simple_relhome_mission(return_path_mission(800)[:3]) + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < 5: + m = self.assert_receive_message('LOCAL_POSITION_NED') + if m.x > 170: + raise NotAchievedException(f"Flew back up the return path to {m.x:.0f}m north") + self.assert_mode_is('AUTO_RTL') + self.change_mode('GUIDED') + self.change_mode('AUTO') + self.wait_current_waypoint(1, timeout=10) + + # a return path with only a located DO_LAND_START still joins at it + self.change_mode('GUIDED') + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2 + (mavutil.mavlink.MAV_CMD_DO_LAND_START, 50, 0, 0), # 3 + (mavutil.mavlink.MAV_CMD_NAV_LAND, 50, 0, 0), # 4 + ]) + self.run_cmd(cmd_return_path_start) + self.assert_mode_is('AUTO_RTL') + self.wait_current_waypoint(4, timeout=10) + + # a leg into a landing is flown at the current altitude, so the + # rejoin altitude is not interpolated down towards the landing + self.change_mode('GUIDED') + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1 + self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2 + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt), # 3 + (mavutil.mavlink.MAV_CMD_NAV_LAND, 50, 0, 0), # 4 + ]) + fly_return_path_from(400, 200, min_alt=alt - 5) + + # the test must finish at home + self.context_pop() + self.do_RTL() + def EK3_OGN_HGT_MASK(self): '''test baraometer-alt-compensation based on long-term GPS readings''' self.context_push() diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 494ea229e1fb..3fe57aaa27cd 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -123,6 +123,16 @@ void AP_Mission::start() void AP_Mission::stop() { _flags.state = MISSION_STOPPED; + + // the vehicle can move away from the rejoin point while the mission is stopped, + // so resume at the end of the leg instead + if (_flags.rejoining_return_path) { + _flags.rejoining_return_path = false; + if (!read_cmd_from_storage(_rejoin_leg_end_index, _nav_cmd)) { + // the leg end has been removed so the mission restarts when resumed + _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; + } + } } /// resume - continues the mission execution from where we last left off @@ -155,6 +165,24 @@ void AP_Mission::resume() return; } + // a return path rejoin point is part way along a leg so it is not in storage, + // if the mission has changed since it was calculated continue to the end of the leg + if (_flags.rejoining_return_path) { + if ((_last_change_time_ms == _rejoin_change_time_ms) && start_command(_rejoin_cmd)) { + _nav_cmd = _rejoin_cmd; + _flags.nav_cmd_loaded = true; + return; + } + // the leg end may have been removed by the mission change + Mission_Command leg_end_cmd; + if (!read_cmd_from_storage(_rejoin_leg_end_index, leg_end_cmd)) { + start(); + return; + } + set_current_cmd(_rejoin_leg_end_index); + return; + } + // rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) { // if not already in a resume state calculate the position to rewind to @@ -272,6 +300,7 @@ void AP_Mission::reset() _flags.do_cmd_all_done = false; _flags.in_landing_sequence = false; _flags.in_return_path = false; + _flags.rejoining_return_path = false; _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE; @@ -637,6 +666,7 @@ bool AP_Mission::set_current_cmd(uint16_t index) // Clear flags _flags.in_landing_sequence = false; _flags.in_return_path = false; + _flags.rejoining_return_path = false; // read command to check for DO_LAND_START and DO_RETURN_PATH_START Mission_Command cmd; @@ -756,6 +786,15 @@ bool AP_Mission::restart_current_nav_cmd() return false; } + // the rejoin point may no longer be on the edited leg so continue to the end of the leg + if (_flags.rejoining_return_path) { + // the leg end may have been removed by the mission change + if (_rejoin_leg_end_index >= num_commands()) { + return false; + } + return set_current_cmd(_rejoin_leg_end_index); + } + return set_current_cmd(_nav_cmd.index); } @@ -2246,6 +2285,7 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index) } // set current navigation command and start it _nav_cmd = cmd; + _flags.rejoining_return_path = false; if (start_command(_nav_cmd)) { _flags.nav_cmd_loaded = true; if (_jump_tag.age > 0 && _jump_tag.age < UINT16_MAX) { @@ -2625,7 +2665,8 @@ bool AP_Mission::jump_to_landing_sequence(const Location ¤t_loc) } /* - find the closest point on the mission after a DO_RETURN_PATH_START and before DO_LAND_START or landing + join the mission at the closest point after a DO_RETURN_PATH_START and before DO_LAND_START or landing + if that point is part way along a leg the vehicle flies to it and then follows the leg */ bool AP_Mission::jump_to_closest_mission_leg(const Location ¤t_loc) { @@ -2642,6 +2683,8 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location ¤t_loc) } uint16_t landing_start_index = 0; + uint16_t rejoin_leg_start_index = AP_MISSION_CMD_INDEX_NONE; + Location rejoin_loc; float min_distance = -1; // This defines the maximum number of waypoints that will be searched, this limits the worst case runtime @@ -2651,10 +2694,14 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location ¤t_loc) for (uint16_t i = 1; i < num_commands(); i++) { if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) { uint16_t tmp_index; + uint16_t tmp_leg_start_index; + Location tmp_rejoin_loc; float tmp_distance; - if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){ + if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, tmp_leg_start_index, tmp_rejoin_loc, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){ min_distance = tmp_distance; landing_start_index = tmp_index; + rejoin_leg_start_index = tmp_leg_start_index; + rejoin_loc = tmp_rejoin_loc; } if (search_remaining == 0) { // Run out of time to search, stop and return the best so far @@ -2663,20 +2710,55 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location ¤t_loc) } } - if (landing_start_index != 0 && set_current_cmd(landing_start_index)) { + if (landing_start_index == 0) { + // failed to find a return path leg + return false; + } - // if the mission has ended it has to be restarted - if (state() == AP_Mission::MISSION_STOPPED) { - resume(); + if (rejoin_leg_start_index != AP_MISSION_CMD_INDEX_NONE) { + // the closest point is part way along a leg + if (!start_return_path_rejoin(rejoin_leg_start_index, landing_start_index, rejoin_loc)) { + return false; } + } else if (!set_current_cmd(landing_start_index)) { + return false; + } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Return path started"); - _flags.in_return_path = true; - return true; + // if the mission has ended it has to be restarted + if (state() == AP_Mission::MISSION_STOPPED) { + resume(); } - // Failed to find do land start - return false; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Return path started"); + _flags.in_return_path = true; + return true; +} + +// start the mission from a virtual wp at rejoin_loc on the leg from leg_start_index to leg_end_index +// the leg's own wps and do commands then follow as normal +bool AP_Mission::start_return_path_rejoin(uint16_t leg_start_index, uint16_t leg_end_index, const Location &rejoin_loc) +{ + // a stopped mission only loads the leg start in set_current_cmd, it does not start it + if (_flags.state == MISSION_RUNNING) { + stop(); + } + if (!set_current_cmd(leg_start_index)) { + return false; + } + _rejoin_leg_end_index = leg_end_index; + _rejoin_change_time_ms = _last_change_time_ms; + + // replace the leg start with a plain wp at the rejoin point, the + // index is kept so the mission continues with the rest of the leg + _rejoin_cmd = {}; + _rejoin_cmd.index = leg_start_index; + _rejoin_cmd.id = MAV_CMD_NAV_WAYPOINT; + _rejoin_cmd.content.location = rejoin_loc; + _nav_cmd = _rejoin_cmd; + _flags.rejoining_return_path = true; + + resume(); + return true; } // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort @@ -2825,15 +2907,50 @@ bool AP_Mission::distance_to_landing(uint16_t index, float &tot_distance, Locati return false; } +// calculate the point the given proportion along the leg from start to end +// returns false if the leg is not straight or its altitude can not be interpolated +bool AP_Mission::calc_rejoin_loc(const Location &start, const Mission_Command &end, const Vector3f &leg, float proportion, Location &rejoin_loc) const +{ + if ((end.id == MAV_CMD_NAV_SPLINE_WAYPOINT) || (end.id == MAV_CMD_NAV_ARC_WAYPOINT)) { + return false; + } + + // landings and zero altitudes are flown at the current altitude + if (is_landing_type_cmd(end.id) || start.alt_is_zero() || end.content.location.alt_is_zero()) { + return false; + } + + const Location::AltFrame frame = start.get_alt_frame(); + int32_t start_alt_cm, end_alt_cm; + if ((end.content.location.get_alt_frame() != frame) || + !start.get_alt_cm(frame, start_alt_cm) || + !end.content.location.get_alt_cm(frame, end_alt_cm)) { + return false; + } + + rejoin_loc = start; + rejoin_loc.offset(leg.x * proportion, leg.y * proportion); + rejoin_loc.set_alt_cm(start_alt_cm + (end_alt_cm - start_alt_cm) * proportion, frame); + return true; +} + // Approximate the distance travelled to return to the mission path. DO_JUMP commands are observed in look forward. // Stop searching once reaching a landing or do-land-start -bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc) +bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, uint16_t &rejoin_leg_start_index, Location &rejoin_loc, const Location& current_loc) { Location prev_loc; + uint16_t prev_index = AP_MISSION_CMD_INDEX_NONE; + bool jumped_since_prev = false; Mission_Command temp_cmd; rejoin_distance = -1; rejoin_index = -1; + rejoin_leg_start_index = AP_MISSION_CMD_INDEX_NONE; bool ret = false; + bool search_complete = false; + + // closest located command that is not flown to, used only if the return path has no nav commands + uint16_t fallback_index = AP_MISSION_CMD_INDEX_NONE; + float fallback_distance = -1; // look ahead on a private copy of the jump tracking so the running mission's jump counters are untouched jump_tracking_array_t jump_state; @@ -2843,15 +2960,31 @@ bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_ uint16_t index = start_index; for (; search_remaining > 0; search_remaining--) { // get the next command; get_next_cmd advances index past it, following any jump taken + const uint16_t scan_start = index; if (!get_next_cmd(index, temp_cmd, jump_state)) { // got to the end of the mission - return ret; + search_complete = true; + break; + } + // get_next_cmd skips over a DO_JUMP, so the command returned is not the one at scan_start + if (temp_cmd.index != scan_start) { + jumped_since_prev = true; } + // only locations the vehicle navigates to form the track if (stored_in_location(temp_cmd.id) && temp_cmd.content.location.initialised()) { - if (prev_loc.lat == 0 && prev_loc.lng == 0) { + if (!is_nav_cmd(temp_cmd)) { + const float distance = temp_cmd.content.location.get_distance_NED_alt_frame(current_loc).length(); + if (is_negative(fallback_distance) || (distance < fallback_distance)) { + fallback_distance = distance; + fallback_index = temp_cmd.index; + } + + } else if (prev_loc.lat == 0 && prev_loc.lng == 0) { // Need a valid previous location to do distance to leg calculation prev_loc = temp_cmd.content.location; + prev_index = temp_cmd.index; + jumped_since_prev = false; // single point dist calc rejoin_distance = prev_loc.get_distance_NED_alt_frame(current_loc).length(); @@ -2864,36 +2997,54 @@ bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_ if (!mission_vector.is_zero()) { Vector3f pos = prev_loc.get_distance_NED_alt_frame(current_loc); - // project pos vector on to mission vector - Vector3f p = pos.projected(mission_vector); - - // constrain to mission line - p.x = constrain_float(p.x, MIN(0,mission_vector.x), MAX(0,mission_vector.x)); - p.y = constrain_float(p.y, MIN(0,mission_vector.y), MAX(0,mission_vector.y)); - p.z = constrain_float(p.z, MIN(0,mission_vector.z), MAX(0,mission_vector.z)); + // proportion along the mission line of the closest point to pos + const float leg_proportion = constrain_float((pos * mission_vector) / mission_vector.length_squared(), 0, 1); + const Vector3f p = mission_vector * leg_proportion; const float disttemp = (p - pos).length(); - // store wp location as previous - prev_loc = temp_cmd.content.location; - if (disttemp < rejoin_distance || is_negative(rejoin_distance)) { rejoin_distance = disttemp; rejoin_index = temp_cmd.index; + rejoin_leg_start_index = AP_MISSION_CMD_INDEX_NONE; + + // a point part way along a leg can be flown to directly, + // otherwise rejoin at the wp at the end of the leg + if ((leg_proportion < 1) && !jumped_since_prev && + calc_rejoin_loc(prev_loc, temp_cmd, mission_vector, leg_proportion, rejoin_loc)) { + rejoin_leg_start_index = prev_index; + } } + ret = true; } + + // store wp location as previous + prev_loc = temp_cmd.content.location; + prev_index = temp_cmd.index; + jumped_since_prev = false; } } if (is_landing_type_cmd(temp_cmd.id) || (temp_cmd.id == MAV_CMD_DO_LAND_START)) { // reached a landing! - return ret; + search_complete = true; + break; } } - // reached end of loop without getting to a landing or DO_LAND_START - return false; + if (!search_complete) { + // reached end of loop without getting to a landing or DO_LAND_START + return false; + } + + if (!ret && (fallback_index != AP_MISSION_CMD_INDEX_NONE)) { + rejoin_distance = fallback_distance; + rejoin_index = fallback_index; + return true; + } + + return ret; } // check if command is a landing type command. diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index c81b345e5635..582c4eec27bb 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -715,7 +715,8 @@ class AP_Mission bool jump_to_abort_landing_sequence(void); #endif - // find the closest point on the mission after a DO_RETURN_PATH_START and before DO_LAND_START or landing + // join the mission at the closest point after a DO_RETURN_PATH_START and before DO_LAND_START or landing + // if that point is part way along a leg the vehicle flies to it and then follows the leg bool jump_to_closest_mission_leg(const Location ¤t_loc); // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan @@ -849,6 +850,7 @@ class AP_Mission bool in_landing_sequence; // true if the mission has jumped to a landing bool resuming_mission; // true if the mission is resuming and set false once the aircraft attains the interrupted WP bool in_return_path; // true if the mission has passed a DO_RETURN_PATH_START waypoint either in the course of the mission or via a `jump_to_closest_mission_leg` call + bool rejoining_return_path; // true while the current nav command is _rejoin_cmd, the virtual wp where the vehicle rejoins a return path leg } _flags; // mission WP resume history @@ -928,7 +930,14 @@ class AP_Mission // Approximate the distance traveled to return to the mission path. DO_JUMP commands are observed in look forward. // Stop searching once reaching a landing or do-land-start - bool distance_to_mission_leg(uint16_t index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc); + // If the closest point is part way along a leg, rejoin_leg_start_index is the index of the leg's first wp and rejoin_loc is the point, otherwise rejoin_leg_start_index is AP_MISSION_CMD_INDEX_NONE + bool distance_to_mission_leg(uint16_t index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, uint16_t &rejoin_leg_start_index, Location &rejoin_loc, const Location& current_loc); + + // calculate the point the given proportion along the leg from start to end, returns false if the leg can not be rejoined part way + bool calc_rejoin_loc(const Location &start, const Mission_Command &end, const Vector3f &leg, float proportion, Location &rejoin_loc) const; + + // start the mission from a virtual wp at rejoin_loc on the leg from leg_start_index to leg_end_index + bool start_return_path_rejoin(uint16_t leg_start_index, uint16_t leg_end_index, const Location &rejoin_loc); // calculate the location of a resume cmd wp bool calc_rewind_pos(Mission_Command& rewind_cmd); @@ -964,6 +973,9 @@ class AP_Mission struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index struct Mission_Command _resume_cmd; // virtual wp command that is used to resume mission if the mission needs to be rewound on resume. + struct Mission_Command _rejoin_cmd; // virtual wp command at the point where the vehicle rejoins a return path leg + uint16_t _rejoin_leg_end_index; // leg end to continue to if the mission changes during the rejoin + uint32_t _rejoin_change_time_ms; // mission change time when the rejoin point was calculated uint16_t _prev_nav_cmd_id; // id of the previous "navigation" command. (WAYPOINT, LOITER_TO_ALT, ect etc) uint16_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command uint16_t _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint. Rarely used which is why we don't store the whole command