aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: GCS_Mavlink: use base class DO_SET_HOME

Browse Source
This commit is contained in:
Iampete1 2022-10-15 17:51:17 +01:00 committed by Andrew Tridgell
parent 7b9126d612
commit da503a5e44
1 changed files with 16 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
ArduPlane/GCS_Mavlink.cpp
View File

@ -727,6 +727,14 @@ bool GCS_MAVLINK_Plane::set_home_to_current_location(bool _lock)
if (_lock) {
AP::ahrs().lock_home();
}
if ((plane.control_mode == &plane.mode_rtl)
#if HAL_QUADPLANE_ENABLED
|| (plane.control_mode == &plane.mode_qrtl)
#endif
) {
// if in RTL head to the updated home location
plane.control_mode->enter();
}
return true;
}
bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool _lock)
@ -737,6 +745,14 @@ 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 HAL_QUADPLANE_ENABLED
|| (plane.control_mode == &plane.mode_qrtl)
#endif
) {
// if in RTL head to the updated home location
plane.control_mode->enter();
}
return true;
}
@ -1062,40 +1078,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_FAILED;
case MAV_CMD_DO_SET_HOME: {
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude (absolute)
if (is_equal(packet.param1,1.0f)) {
if (!plane.set_home_persistently(AP::gps().location())) {
return MAV_RESULT_FAILED;
}
AP::ahrs().lock_home();
} else {
// ensure param1 is zero
if (!is_zero(packet.param1)) {
return MAV_RESULT_FAILED;
}
Location new_home_loc;
if (!location_from_command_t(packet, MAV_FRAME_GLOBAL, new_home_loc)) {
return MAV_RESULT_DENIED;
}
if (!set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED;
}
}
if ((plane.control_mode == &plane.mode_rtl)
#if HAL_QUADPLANE_ENABLED
|| (plane.control_mode == &plane.mode_qrtl)
#endif
) {
// if in RTL head to the updated home location
plane.control_mode->enter();
}
return MAV_RESULT_ACCEPTED;
}
case MAV_CMD_DO_AUTOTUNE_ENABLE:
// param1 : enable/disable
plane.autotune_enable(!is_zero(packet.param1));