Browse Source

Plane: make FENCE_AUTOENABLE an AP_Enum

zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
52f61c7ac1
  1. 4
      ArduPlane/AP_Arming.cpp
  2. 2
      ArduPlane/Parameters.h
  3. 4
      ArduPlane/commands_logic.cpp
  4. 7
      ArduPlane/defines.h
  5. 2
      ArduPlane/takeoff.cpp

4
ArduPlane/AP_Arming.cpp

@ -151,7 +151,7 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method) @@ -151,7 +151,7 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
}
#if GEOFENCE_ENABLED == ENABLED
if (plane.g.fence_autoenable == 3) {
if (plane.g.fence_autoenable == FenceAutoEnable::WhenArmed) {
if (!plane.geofence_set_enabled(true)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence: cannot enable for arming");
return false;
@ -239,7 +239,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method) @@ -239,7 +239,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method)
gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");
#if GEOFENCE_ENABLED == ENABLED
if (plane.g.fence_autoenable == 3) {
if (plane.g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
plane.geofence_set_enabled(false);
}
#endif

2
ArduPlane/Parameters.h

@ -401,7 +401,7 @@ public: @@ -401,7 +401,7 @@ public:
AP_Int16 fence_minalt; // meters
AP_Int16 fence_maxalt; // meters
AP_Int16 fence_retalt; // meters
AP_Int8 fence_autoenable;
AP_Enum<FenceAutoEnable> fence_autoenable;
AP_Int8 fence_ret_rally;
#endif

4
ArduPlane/commands_logic.cpp

@ -386,13 +386,13 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) @@ -386,13 +386,13 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
}
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
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 == 2) {
} 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 {

7
ArduPlane/defines.h

@ -172,3 +172,10 @@ enum class AirMode { @@ -172,3 +172,10 @@ enum class AirMode {
OFF,
ON,
};
enum class FenceAutoEnable : uint8_t {
OFF=0,
Auto=1,
AutoDisableFloorOnly=2,
WhenArmed=3
};

2
ArduPlane/takeoff.cpp

@ -272,7 +272,7 @@ return_zero: @@ -272,7 +272,7 @@ return_zero:
void Plane::complete_auto_takeoff(void)
{
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable > 0) {
if (g.fence_autoenable != FenceAutoEnable::OFF) {
if (! geofence_set_enabled(true)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Enable fence failed (cannot autoenable");
} else {

Loading…
Cancel
Save