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

Plane: Allow disabling of only the fence floor.

Browse Source
This commit is contained in:
Michael Day 2015-04-09 10:02:38 -06:00 committed by Andrew Tridgell
parent b7ebff409b
commit ee14678d2a
5 changed files with 49 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.pde
View File

@ -1496,6 +1496,12 @@ static void set_flight_stage(AP_SpdHgtControl::FlightStage fs)
} else {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence disabled (autodisable)"));
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence floor failed (autodisable)"));
} else {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence floor disabled (auto disable)"));
}
}
#endif
}

6
ArduPlane/GCS_Mavlink.pde
View File

@ -1220,6 +1220,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_FAILED;
}
break;
case 2: //disable fence floor only
if (! geofence_set_floor_enabled(false)) {
result = MAV_RESULT_FAILED;
} else {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Fence floor disabled."));
}
default:
result = MAV_RESULT_FAILED;
break;

4
ArduPlane/Parameters.pde
View File

@ -358,8 +358,8 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FENCE_AUTOENABLE
// @DisplayName: Fence automatic enable
// @Description: When set to 1, gefence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead.
// @Values: 0:NoAutoEnable,1:AutoEnable
// @Description: When set to 1, gefence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead.
// @Values: 0:NoAutoEnable,1:AutoEnable,2:AutoEnableDisableFloorOnly
// @User: Standard
GSCALAR(fence_autoenable, "FENCE_AUTOENABLE", 0),

18
ArduPlane/commands_logic.pde
View File

@ -148,10 +148,18 @@ start_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_DO_FENCE_ENABLE:
#if GEOFENCE_ENABLED == ENABLED
if (!geofence_set_enabled((bool) cmd.p1, AUTO_TOGGLED)) {
gcs_send_text_fmt(PSTR("Unable to set fence enabled state to %u"), cmd.p1);
} else {
gcs_send_text_fmt(PSTR("Set fence enabled state to %u"), cmd.p1);
if (cmd.p1 != 2) {
if (!geofence_set_enabled((bool) cmd.p1, AUTO_TOGGLED)) {
gcs_send_text_fmt(PSTR("Unable to set fence enabled state to %u"), cmd.p1);
} else {
gcs_send_text_fmt(PSTR("Set fence enabled state to %u"), cmd.p1);
}
} else { //commanding to only disable floor
if (! geofence_set_floor_enabled(false)) {
gcs_send_text_fmt(PSTR("Unabled to disable fence floor.\n"));
} else {
gcs_send_text_fmt(PSTR("Fence floor disabled.\n"));
}
}
#endif
break;
@ -447,7 +455,7 @@ static bool verify_takeoff()
next_WP_loc = prev_WP_loc = current_loc;
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (g.fence_autoenable > 0) {
if (! geofence_set_enabled(true, AUTO_TOGGLED)) {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Enable fence failed (cannot autoenable"));
} else {

23
ArduPlane/geofence.pde
View File

@ -28,6 +28,7 @@ static struct GeofenceState {
// last time we checked
bool is_enabled;
GeofenceEnableReason enable_reason;
bool floor_enabled; //typically used for landing
uint16_t breach_count;
uint8_t breach_type;
uint32_t breach_time;
@ -197,6 +198,10 @@ static bool geofence_set_enabled(bool enable, GeofenceEnableReason r)
}
geofence_state->is_enabled = enable;
if (enable == true) {
//turn the floor back on if it had been off
geofence_set_floor_enabled(true);
}
geofence_state->enable_reason = r;
return true;
@ -225,6 +230,18 @@ static bool geofence_enabled(void)
return true;
}
/*
* Set floor state IF the fence is present.
* Return false on failure to set floor state.
*/
static bool geofence_set_floor_enabled(bool floor_enable) {
if (geofence_state == NULL) {
return false;
}
geofence_state->floor_enabled = floor_enable;
return true;
}
/*
* return true if we have breached the geo-fence minimum altiude
@ -295,7 +312,7 @@ static void geofence_check(bool altitude_check_only)
return;
}
if (geofence_check_minalt()) {
if (geofence_state->floor_enabled && geofence_check_minalt()) {
outside = true;
breach_type = FENCE_BREACH_MINALT;
} else if (geofence_check_maxalt()) {
@ -457,6 +474,10 @@ static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) {
return false;
}
static bool geofence_set_floor_enabled(bool floor_enable) {
return false;
}
bool geofence_breached(void) {
return false;
}