mirror of https://github.com/ArduPilot/ardupilot
ArduPlane: move handling of DO_SET_HOME up to GCS_MAVLink base class
This commit is contained in:
parent
5d41125b49
commit
42ce0867ae
|
@ -781,25 +781,25 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
|
|||
}
|
||||
|
||||
|
||||
bool GCS_MAVLINK_Plane::set_home_to_current_location(bool _lock)
|
||||
bool Plane::set_home_to_current_location(bool _lock)
|
||||
{
|
||||
if (!plane.set_home_persistently(AP::gps().location())) {
|
||||
if (!set_home_persistently(AP::gps().location())) {
|
||||
return false;
|
||||
}
|
||||
if (_lock) {
|
||||
AP::ahrs().lock_home();
|
||||
}
|
||||
if ((plane.control_mode == &plane.mode_rtl)
|
||||
if ((control_mode == &mode_rtl)
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
|| (plane.control_mode == &plane.mode_qrtl)
|
||||
|| (control_mode == &mode_qrtl)
|
||||
#endif
|
||||
) {
|
||||
// if in RTL head to the updated home location
|
||||
plane.control_mode->enter();
|
||||
control_mode->enter();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool _lock)
|
||||
bool Plane::set_home(const Location& loc, bool _lock)
|
||||
{
|
||||
if (!AP::ahrs().set_home(loc)) {
|
||||
return false;
|
||||
|
@ -807,13 +807,13 @@ bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool _lock)
|
|||
if (_lock) {
|
||||
AP::ahrs().lock_home();
|
||||
}
|
||||
if ((plane.control_mode == &plane.mode_rtl)
|
||||
if ((control_mode == &mode_rtl)
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
|| (plane.control_mode == &plane.mode_qrtl)
|
||||
|| (control_mode == &mode_qrtl)
|
||||
#endif
|
||||
) {
|
||||
// if in RTL head to the updated home location
|
||||
plane.control_mode->enter();
|
||||
control_mode->enter();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
|
|
@ -42,8 +42,6 @@ protected:
|
|||
|
||||
bool persist_streamrates() const override { return true; }
|
||||
|
||||
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
|
||||
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
|
||||
uint64_t capabilities() const override;
|
||||
|
||||
void send_nav_controller_output() const override;
|
||||
|
|
|
@ -986,6 +986,8 @@ private:
|
|||
|
||||
// set home location and store it persistently:
|
||||
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
|
||||
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
|
||||
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
|
||||
|
||||
// control_modes.cpp
|
||||
void read_control_switch();
|
||||
|
|
Loading…
Reference in New Issue