From 53db811626cd34564382db29607a91bc8e510dae Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Thu, 7 Mar 2024 09:38:03 -0800 Subject: [PATCH] Sub: support ABOVE_TERRAIN frame in auto --- ArduSub/commands_logic.cpp | 23 ++++++++++++++--------- ArduSub/mode_auto.cpp | 1 + 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 40a6b70f4d..392749c6cf 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -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; } diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index d947c8ed0e..57e0861189 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -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; }