Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: handle_guided_request per mode

This commit is contained in:
Iampete1 2021-09-27 20:28:39 +01:00 committed by Andrew Tridgell
parent 1148bb1234
commit a0260b05b3
3 changed files with 23 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/GCS_Mavlink.cpp
View File

@ -604,20 +604,7 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
*/
bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
{
if (plane.control_mode != &plane.mode_guided) {
// only accept position updates when in GUIDED mode
return false;
}
plane.guided_WP_loc = cmd.content.location;
// add home alt if needed
if (plane.guided_WP_loc.relative_alt) {
plane.guided_WP_loc.alt += plane.home.alt;
plane.guided_WP_loc.relative_alt = 0;
}
plane.set_guided_WP();
return true;
return plane.control_mode->handle_guided_request(cmd.content.location);
}
/*

8
ArduPlane/mode.h
View File

@ -118,7 +118,10 @@ public:
// method for mode specific target altitude profiles
virtual bool update_target_altitude() { return false; }
// handle a guided target request from GCS
virtual bool handle_guided_request(Location target_loc) { return false; }
protected:
// subclasses override this to perform checks before entering the mode
@ -220,6 +223,9 @@ public:
bool does_auto_throttle() const override { return true; }
// handle a guided target request from GCS
bool handle_guided_request(Location target_loc) override;
protected:
bool _enter() override;

15
ArduPlane/mode_guided.cpp
View File

@ -42,3 +42,18 @@ void ModeGuided::navigate()
// Zero indicates to use WP_LOITER_RAD
plane.update_loiter(0);
}
bool ModeGuided::handle_guided_request(Location target_loc)
{
plane.guided_WP_loc = target_loc;
// add home alt if needed
if (plane.guided_WP_loc.relative_alt) {
plane.guided_WP_loc.alt += plane.home.alt;
plane.guided_WP_loc.relative_alt = 0;
}
plane.set_guided_WP();
return true;
}