From b7c937592edef8e2b665bb32c6f61aa9e01c11ff Mon Sep 17 00:00:00 2001 From: chobits Date: Fri, 5 Mar 2021 15:03:56 +0800 Subject: [PATCH] Copter: send mission_item_reached in guided --- ArduCopter/mode.h | 1 + ArduCopter/mode_guided.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1cc8ccfcbb..5f152f6ccd 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -849,6 +849,7 @@ private: // controls which controller is run (pos or vel): GuidedMode guided_mode = Guided_TakeOff; + bool send_notification; // used to send one time notification to ground station }; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 2e7664a204..0f79ee3d2d 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -44,6 +44,7 @@ bool ModeGuided::init(bool ignore_checks) { // start in position control mode pos_control_start(); + send_notification = false; return true; } @@ -62,6 +63,10 @@ void ModeGuided::run() case Guided_WP: // run position controller pos_control_run(); + if (send_notification && wp_nav->reached_wp_destination()) { + send_notification = false; + gcs().send_mission_item_reached_message(0); + } break; case Guided_Velocity: @@ -257,6 +262,9 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // log target copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); + + send_notification = true; + return true; } @@ -300,6 +308,9 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // log target copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + + send_notification = true; + return true; }