From d02d8979288d64f71f943277f6f42f0049647f56 Mon Sep 17 00:00:00 2001 From: Arjun Vinod Date: Mon, 25 Feb 2019 15:38:38 -0500 Subject: [PATCH] Copter: make LOITER_TIME send "Reached command" message to GCS --- ArduCopter/mode.h | 2 +- ArduCopter/mode_auto.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dcf6dbbbcd..18502ea122 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -388,7 +388,7 @@ private: bool verify_land(); bool verify_payload_place(); bool verify_loiter_unlimited(); - bool verify_loiter_time(); + bool verify_loiter_time(const AP_Mission::Mission_Command& cmd); bool verify_loiter_to_alt(); bool verify_RTL(); bool verify_wait_delay(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f9d33a672d..98e5f7a855 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -629,7 +629,7 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_LOITER_TIME: - cmd_complete = verify_loiter_time(); + cmd_complete = verify_loiter_time(cmd); break; case MAV_CMD_NAV_LOITER_TO_ALT: @@ -1656,7 +1656,7 @@ bool Copter::ModeAuto::verify_loiter_unlimited() } // verify_loiter_time - check if we have loitered long enough -bool Copter::ModeAuto::verify_loiter_time() +bool Copter::ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) { // return immediately if we haven't reached our destination if (!copter.wp_nav->reached_wp_destination()) { @@ -1669,7 +1669,12 @@ bool Copter::ModeAuto::verify_loiter_time() } // check if loiter timer has run out - return (((millis() - loiter_time) / 1000) >= loiter_time_max); + if (((millis() - loiter_time) / 1000) >= loiter_time_max) { + gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); + return true; + } + + return false; } // verify_loiter_to_alt - check if we have reached both destination