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

Sub: support ABOVE_TERRAIN frame in auto

This commit is contained in:
Clyde McQueen 2024-03-07 09:38:03 -08:00 committed by Willian Galvani
parent 1f8ab27996
commit 858376bd86
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduSub/commands_logic.cpp
View File

@ -15,16 +15,20 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
#endif
const Location &target_loc = cmd.content.location;
auto alt_frame = target_loc.get_alt_frame();
// target alt must be negative (underwater)
if (target_loc.alt > 0.0f) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
return false;
}
// only tested/supported alt frame so far is AltFrame::ABOVE_HOME, where Home alt is always water's surface ie zero depth
if (target_loc.get_alt_frame() != Location::AltFrame::ABOVE_HOME) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV AltFrame %d", (int8_t)target_loc.get_alt_frame());
if (alt_frame == Location::AltFrame::ABOVE_HOME) {
if (target_loc.alt > 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Alt above home must be negative");
return false;
}
} else if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
if (target_loc.alt < 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Alt above terrain must be positive");
return false;
}
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Bad alt frame");
return false;
}
@ -111,6 +115,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
default:
// unable to use the command, allow the vehicle to try the next command
gcs().send_text(MAV_SEVERITY_WARNING, "Ignoring command %d", cmd.id);
return false;
}

1
ArduSub/mode_auto.cpp
View File

@ -88,6 +88,7 @@ void ModeAuto::auto_wp_start(const Location& dest_loc)
// send target to waypoint controller
if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
gcs().send_text(MAV_SEVERITY_WARNING, "Terrain data (rangefinder) not available");
sub.failsafe_terrain_on_event();
return;
}