From 2b8958f93143de5da1e67288b05e4b8859d61c8b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 4 Mar 2022 16:36:36 +0000 Subject: [PATCH] Plane: Fence moved to vehicle --- ArduPlane/ArduPlane.cpp | 3 --- ArduPlane/Parameters.cpp | 13 +++++++------ ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 8 -------- ArduPlane/config.h | 5 ----- ArduPlane/quadplane.h | 1 - ArduPlane/system.cpp | 5 ----- 7 files changed, 8 insertions(+), 29 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a627ae17e1..8d4310a499 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -78,9 +78,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66), SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150, 69), SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300, 72), -#if AC_FENCE == ENABLED - SCHED_TASK_CLASS(AC_Fence, &plane.fence, update, 10, 100, 75), -#endif SCHED_TASK(read_rangefinder, 50, 100, 78), #if AP_ICENGINE_ENABLED SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81), diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index a06edc543c..fffc5ed58e 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -950,12 +950,6 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Rally/AP_Rally.cpp GOBJECT(rally, "RALLY_", AP_Rally), -#if AC_FENCE == ENABLED - // @Group: FENCE_ - // @Path: ../libraries/AC_Fence/AC_Fence.cpp - GOBJECT(fence, "FENCE_", AC_Fence), -#endif - #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1391,6 +1385,7 @@ void Plane::load_parameters(void) } } +#if AC_FENCE enum ap_var_type ptype_fence_type; AP_Int8 *fence_type_new = (AP_Int8*)AP_Param::find("FENCE_TYPE", &ptype_fence_type); if (fence_type_new && !fence_type_new->configured()) { @@ -1475,6 +1470,7 @@ void Plane::load_parameters(void) } } } +#endif // AC_FENCE #if AP_TERRAIN_AVAILABLE g.terrain_follow.convert_parameter_width(AP_PARAM_INT8); @@ -1526,5 +1522,10 @@ void Plane::load_parameters(void) } #endif // HAL_INS_NUM_HARMONIC_NOTCH_FILTERS + // PARAMETER_CONVERSION - Added: Mar-2022 +#if AC_FENCE + AP_Param::convert_class(g.k_param_fence, &fence, fence.var_info, 0, 0, true); +#endif + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index cf32065386..ab94503a9d 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -350,7 +350,7 @@ public: k_param_gcs4, // stream rates k_param_gcs5, // stream rates k_param_gcs6, // stream rates - k_param_fence, // vehicle fence + k_param_fence, // vehicle fence - unused k_param_acro_yaw_rate, }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d3de334c3b..3ddaa0aba0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -97,10 +97,6 @@ #include "afs_plane.h" #endif -#if AC_FENCE == ENABLED -#include -#endif - // Local modules #include "defines.h" #include "mode.h" @@ -254,10 +250,6 @@ private: AP_OSD osd; #endif -#if AC_FENCE == ENABLED - AC_Fence fence; -#endif - ModeCircle mode_circle; ModeStabilize mode_stabilize; ModeTraining mode_training; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index f27263f074..e0c1283a1f 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -233,11 +233,6 @@ # define SCALING_SPEED 15.0 #endif -// use this to disable geo-fencing -#ifndef AC_FENCE - # define AC_FENCE ENABLED -#endif - // a digital pin to set high when the geo-fence triggers. Defaults // to -1, which means don't activate a pin #ifndef FENCE_TRIGGERED_PIN diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 478975faae..f3edc81859 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index ed7e856718..eac7fb238f 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -147,11 +147,6 @@ void Plane::init_ardupilot() #if GRIPPER_ENABLED == ENABLED g2.gripper.init(); #endif - - // init fence -#if AC_FENCE == ENABLED - fence.init(); -#endif } //********************************************************************************