Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed disable of geofence on quadplane landing

Browse Source 
fixes #15917
This commit is contained in:
Andrew Tridgell 2020-12-01 14:22:34 +11:00
parent 52f61c7ac1
commit cd6ddf7d4f
5 changed files with 26 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/AP_Arming.cpp
View File

@ -239,7 +239,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method)
gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed"); gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");
#if GEOFENCE_ENABLED == ENABLED #if GEOFENCE_ENABLED == ENABLED
if (plane.g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) { if (plane.g.fence_autoenable == FenceAutoEnable::WhenArmed) {
plane.geofence_set_enabled(false); plane.geofence_set_enabled(false);
} }
#endif #endif

1
ArduPlane/Plane.h
View File

@ -949,6 +949,7 @@ private:
void geofence_send_status(mavlink_channel_t chan); void geofence_send_status(mavlink_channel_t chan);
bool geofence_breached(void); bool geofence_breached(void);
void geofence_disable_and_send_error_msg(const char *errorMsg); void geofence_disable_and_send_error_msg(const char *errorMsg);
void disable_fence_for_landing(void);
// ArduPlane.cpp // ArduPlane.cpp
void disarm_if_autoland_complete(); void disarm_if_autoland_complete();

16
ArduPlane/commands_logic.cpp
View File

@ -385,21 +385,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
} }
#if GEOFENCE_ENABLED == ENABLED disable_fence_for_landing();
if (g.fence_autoenable == FenceAutoEnable::Auto) {
if (! geofence_set_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
if (! geofence_set_floor_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
} }
void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd) void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)

21
ArduPlane/geofence.cpp
View File

@ -513,6 +513,23 @@ bool Plane::geofence_breached(void)
return geofence_state ? geofence_state->fence_triggered : false; return geofence_state ? geofence_state->fence_triggered : false;
} }
void Plane::disable_fence_for_landing(void)
{
if (g.fence_autoenable == FenceAutoEnable::Auto) {
if (!geofence_set_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
if (!geofence_set_floor_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
}
#else // GEOFENCE_ENABLED #else // GEOFENCE_ENABLED
@ -542,4 +559,8 @@ bool Plane::geofence_breached(void)
return false; return false;
} }
void Plane::disable_fence_for_landing(void)
{
}
#endif // GEOFENCE_ENABLED #endif // GEOFENCE_ENABLED

2
ArduPlane/quadplane.cpp
View File

@ -1201,6 +1201,7 @@ void QuadPlane::init_qland(void)
#if LANDING_GEAR_ENABLED == ENABLED #if LANDING_GEAR_ENABLED == ENABLED
plane.g2.landing_gear.deploy_for_landing(); plane.g2.landing_gear.deploy_for_landing();
#endif #endif
plane.disable_fence_for_landing();
} }
@ -2929,6 +2930,7 @@ bool QuadPlane::verify_vtol_land(void)
if (poscontrol.state == QPOS_POSITION2 && if (poscontrol.state == QPOS_POSITION2 &&
plane.auto_state.wp_distance < 2) { plane.auto_state.wp_distance < 2) {
poscontrol.state = QPOS_LAND_DESCEND; poscontrol.state = QPOS_LAND_DESCEND;
plane.disable_fence_for_landing();
#if LANDING_GEAR_ENABLED == ENABLED #if LANDING_GEAR_ENABLED == ENABLED
plane.g2.landing_gear.deploy_for_landing(); plane.g2.landing_gear.deploy_for_landing();
#endif #endif