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 6534a9657c
commit 53db811626
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduSub/commands_logic.cpp
View File

@ -15,16 +15,20 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
#endif #endif
const Location &target_loc = cmd.content.location; const Location &target_loc = cmd.content.location;
auto alt_frame = target_loc.get_alt_frame();
// target alt must be negative (underwater) if (alt_frame == Location::AltFrame::ABOVE_HOME) {
if (target_loc.alt > 0.0f) { if (target_loc.alt > 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt); gcs().send_text(MAV_SEVERITY_WARNING, "Alt above home must be negative");
return false; return false;
} }
} else if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
// 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.alt < 0) {
if (target_loc.get_alt_frame() != Location::AltFrame::ABOVE_HOME) { gcs().send_text(MAV_SEVERITY_WARNING, "Alt above terrain must be positive");
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV AltFrame %d", (int8_t)target_loc.get_alt_frame()); return false;
}
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Bad alt frame");
return false; return false;
} }
@ -111,6 +115,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
default: default:
// unable to use the command, allow the vehicle to try the next command // 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; 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 // send target to waypoint controller
if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data // 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(); sub.failsafe_terrain_on_event();
return; return;
} }