Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: support `DO_RETURN_PATH_START`

This commit is contained in:
Iampete1 2024-03-03 00:12:23 +00:00 committed by Randy Mackay
parent e8d2097ec4
commit 5b9848c5e1
4 changed files with 84 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -836,6 +836,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
return MAV_RESULT_ACCEPTED;
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_DO_RETURN_PATH_START:
if (copter.mode_auto.return_path_start_auto_RTL(ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
case MAV_CMD_DO_LAND_START:
if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;

3
ArduCopter/mode.cpp
View File

@ -276,7 +276,8 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
#if MODE_AUTO_ENABLED == ENABLED
if (mode == Mode::Number::AUTO_RTL) {
// Special case for AUTO RTL, not a true mode, just AUTO in disguise
return mode_auto.jump_to_landing_sequence_auto_RTL(reason);
// Attempt to join return path, fallback to do-land-start
return mode_auto.return_path_or_jump_to_landing_sequence_auto_RTL(reason);
}
#endif

9
ArduCopter/mode.h
View File

@ -553,6 +553,12 @@ public:
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
// Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
bool return_path_start_auto_RTL(ModeReason reason);
// Try join return path else do land start
bool return_path_or_jump_to_landing_sequence_auto_RTL(ModeReason reason);
// lua accessors for nav script time support
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4);
void nav_script_time_done(uint16_t id);
@ -589,6 +595,9 @@ private:
AllowWeatherVaning = (1 << 7U),
};
// Enter auto rtl pseudo mode
bool enter_auto_rtl(ModeReason reason);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
void exit_mission();

86
ArduCopter/mode_auto.cpp
View File

@ -164,7 +164,8 @@ void ModeAuto::run()
}
// only pretend to be in auto RTL so long as mission still thinks its in a landing sequence or the mission has completed
if (auto_RTL && (!(mission.get_in_landing_sequence_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE))) {
const bool auto_rtl_active = mission.get_in_landing_sequence_flag() || mission.get_in_return_path_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE;
if (auto_RTL && !auto_rtl_active) {
auto_RTL = false;
// log exit from Auto RTL
#if HAL_LOGGING_ENABLED
@ -216,30 +217,75 @@ bool ModeAuto::allows_weathervaning() const
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
{
if (mission.jump_to_landing_sequence(get_stopping_point())) {
mission.set_force_resume(true);
// if not already in auto switch to auto
if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) {
auto_RTL = true;
if (!mission.jump_to_landing_sequence(get_stopping_point())) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
// make sad noise
if (copter.ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1;
}
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
return false;
}
return enter_auto_rtl(reason);
}
// Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
bool ModeAuto::return_path_start_auto_RTL(ModeReason reason)
{
if (!mission.jump_to_closest_mission_leg(get_stopping_point())) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
// make sad noise
if (copter.ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1;
}
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No return path found");
return false;
}
return enter_auto_rtl(reason);
}
// Try join return path else do land start
bool ModeAuto::return_path_or_jump_to_landing_sequence_auto_RTL(ModeReason reason)
{
const Location stopping_point = get_stopping_point();
if (!mission.jump_to_closest_mission_leg(stopping_point) && !mission.jump_to_landing_sequence(stopping_point)) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
// make sad noise
if (copter.ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1;
}
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No return path or landing sequence found");
return false;
}
return enter_auto_rtl(reason);
}
// Enter auto rtl pseudo mode
bool ModeAuto::enter_auto_rtl(ModeReason reason)
{
mission.set_force_resume(true);
// if not already in auto switch to auto
if ((copter.flightmode == this) || set_mode(Mode::Number::AUTO, reason)) {
auto_RTL = true;
#if HAL_LOGGING_ENABLED
// log entry into AUTO RTL
copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
// log entry into AUTO RTL
copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
#endif
// make happy noise
if (copter.ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
return true;
// make happy noise
if (copter.ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
// mode change failed, revert force resume flag
mission.set_force_resume(false);
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed");
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
return true;
}
// mode change failed, revert force resume flag
mission.set_force_resume(false);
LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
// make sad noise
if (copter.ap.initialised) {
@ -758,6 +804,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
break;
#endif
case MAV_CMD_DO_RETURN_PATH_START:
case MAV_CMD_DO_LAND_START:
break;
@ -964,6 +1011,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_DO_GUIDED_LIMITS:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_WINCH:
case MAV_CMD_DO_RETURN_PATH_START:
case MAV_CMD_DO_LAND_START:
cmd_complete = true;
break;