Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: send mission_item_reached in guided

Browse Source
This commit is contained in:
chobits 2021-03-05 15:03:56 +08:00 committed by Randy Mackay
parent 29e85aa516
commit b7c937592e
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -849,6 +849,7 @@ private:
// controls which controller is run (pos or vel): // controls which controller is run (pos or vel):
GuidedMode guided_mode = Guided_TakeOff; GuidedMode guided_mode = Guided_TakeOff;
bool send_notification; // used to send one time notification to ground station
}; };

11
ArduCopter/mode_guided.cpp
View File

@ -44,6 +44,7 @@ bool ModeGuided::init(bool ignore_checks)
{ {
// start in position control mode // start in position control mode
pos_control_start(); pos_control_start();
send_notification = false;
return true; return true;
} }
@ -62,6 +63,10 @@ void ModeGuided::run()
case Guided_WP: case Guided_WP:
// run position controller // run position controller
pos_control_run(); pos_control_run();
if (send_notification && wp_nav->reached_wp_destination()) {
send_notification = false;
gcs().send_mission_item_reached_message(0);
}
break; break;
case Guided_Velocity: case Guided_Velocity:
@ -257,6 +262,9 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
send_notification = true;
return true; return true;
} }
@ -300,6 +308,9 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
// log target // log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
send_notification = true;
return true; return true;
} }