From eac067a5c4465f0bc423bb74b3cad455d2898201 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 4 Mar 2022 16:41:00 +0000 Subject: [PATCH] Rover: fence moved to vehicle --- Rover/AP_Rally.cpp | 4 +++- Rover/Parameters.cpp | 11 ++++++----- Rover/Parameters.h | 3 --- Rover/Rover.cpp | 1 - Rover/failsafe.cpp | 2 +- Rover/fence.cpp | 12 +++++++----- Rover/mode_auto.cpp | 6 ++++-- Rover/system.cpp | 6 +++--- Rover/wscript | 1 - 9 files changed, 24 insertions(+), 22 deletions(-) diff --git a/Rover/AP_Rally.cpp b/Rover/AP_Rally.cpp index 4442fee50c..4680dda12f 100644 --- a/Rover/AP_Rally.cpp +++ b/Rover/AP_Rally.cpp @@ -21,8 +21,10 @@ bool AP_Rally_Rover::is_valid(const Location &rally_point) const { - if (!rover.g2.fence.check_destination_within_fence(rally_point)) { +#if AC_FENCE + if (!rover.fence.check_destination_within_fence(rally_point)) { return false; } +#endif return true; } diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 739dfa3c3f..542ee8ae23 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -478,10 +478,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("FRAME_CLASS", 16, ParametersG2, frame_class, 1), - // @Group: FENCE_ - // @Path: ../libraries/AC_Fence/AC_Fence.cpp - AP_SUBGROUPINFO(fence, "FENCE_", 17, ParametersG2, AC_Fence), - #if HAL_PROXIMITY_ENABLED // @Group: PRX // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp @@ -847,7 +843,7 @@ void Rover::load_parameters(void) AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED); #endif -#if AP_AIRSPEED_ENABLED | AP_AIS_ENABLED +#if AP_AIRSPEED_ENABLED | AP_AIS_ENABLED | AC_FENCE // Find G2's Top Level Key AP_Param::ConversionInfo info; if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { @@ -866,4 +862,9 @@ void Rover::load_parameters(void) #if AP_AIS_ENABLED AP_Param::convert_class(info.old_key, &ais, ais.var_info, 50, 114, false); #endif + +// PARAMETER_CONVERSION - Added: Mar-2022 +#if AC_FENCE + AP_Param::convert_class(info.old_key, &fence, fence.var_info, 17, 4049, false); +#endif } diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 89b506fc99..d18e32f5f5 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -334,9 +334,6 @@ public: // frame class for vehicle AP_Int8 frame_class; - // fence library - AC_Fence fence; - #if HAL_PROXIMITY_ENABLED // proximity library AP_Proximity proximity; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index c8d656752a..0b2f28033d 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -82,7 +82,6 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200, 27), #endif SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100, 30), - SCHED_TASK_CLASS(AC_Fence, &rover.g2.fence, update, 10, 100, 33), SCHED_TASK(update_wheel_encoder, 50, 200, 36), SCHED_TASK(update_compass, 10, 200, 39), SCHED_TASK(update_logging1, 10, 200, 45), diff --git a/Rover/failsafe.cpp b/Rover/failsafe.cpp index b509868822..68a2f3ca0c 100644 --- a/Rover/failsafe.cpp +++ b/Rover/failsafe.cpp @@ -149,6 +149,6 @@ void Rover::handle_battery_failsafe(const char* type_str, const int8_t action) void Rover::afs_fs_check(void) { // perform AFS failsafe checks - g2.afs.check(g2.fence.get_breaches() != 0, failsafe.last_valid_rc_ms); + g2.afs.check(fence.get_breaches() != 0, failsafe.last_valid_rc_ms); } #endif diff --git a/Rover/fence.cpp b/Rover/fence.cpp index 84722f4d1b..d541b92be7 100644 --- a/Rover/fence.cpp +++ b/Rover/fence.cpp @@ -3,11 +3,12 @@ // fence_check - ask fence library to check for breaches and initiate the response void Rover::fence_check() { +#if AC_FENCE uint8_t new_breaches; // the type of fence that has been breached - const uint8_t orig_breaches = g2.fence.get_breaches(); + const uint8_t orig_breaches = fence.get_breaches(); // check for a breach - new_breaches = g2.fence.check(); + new_breaches = fence.check(); // return immediately if motors are not armed if (!arming.is_armed()) { @@ -17,10 +18,10 @@ void Rover::fence_check() // if there is a new breach take action if (new_breaches) { // if the user wants some kind of response and motors are armed - if (g2.fence.get_action() != Failsafe_Action_None) { + if (fence.get_action() != Failsafe_Action_None) { // if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter - if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { - switch (g2.fence.get_action()) { + if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { + switch (fence.get_action()) { case Failsafe_Action_None: break; case Failsafe_Action_RTL: @@ -56,4 +57,5 @@ void Rover::fence_check() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } +#endif // AC_FENCE } diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 5cd5182d23..4900a02232 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -481,13 +481,15 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_DO_FENCE_ENABLE: +#if AC_FENCE if (cmd.p1 == 0) { //disable - g2.fence.enable(false); + rover.fence.enable(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); } else { //enable fence - g2.fence.enable(true); + rover.fence.enable(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); } +#endif break; case MAV_CMD_DO_GUIDED_LIMITS: diff --git a/Rover/system.cpp b/Rover/system.cpp index 709911dc53..eb5065798e 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -29,8 +29,6 @@ void Rover::init_ardupilot() g2.gripper.init(); #endif - g2.fence.init(); - // initialise notify system notify.init(); notify_mode(control_mode); @@ -222,10 +220,12 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) control_mode = &new_mode; +#if AC_FENCE // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) // but it should be harmless to disable the fence temporarily in these situations as well - g2.fence.manual_recovery_start(); + fence.manual_recovery_start(); +#endif #if CAMERA == ENABLED camera.set_is_auto_mode(control_mode->mode_number() == Mode::Number::AUTO); diff --git a/Rover/wscript b/Rover/wscript index 980299e852..5d887bd32c 100644 --- a/Rover/wscript +++ b/Rover/wscript @@ -17,7 +17,6 @@ def build(bld): 'AP_AdvancedFailsafe', 'AP_WheelEncoder', 'AP_SmartRTL', - 'AC_Fence', 'AC_AttitudeControl', 'AP_LTM_Telem', 'AP_Devo_Telem',