From c3fb985ec5b0b67968e7512d3bdb13bea5747ec4 Mon Sep 17 00:00:00 2001 From: khancyr Date: Wed, 16 Aug 2017 12:02:56 +0200 Subject: [PATCH] Rover: add fence support --- APMrover2/APMrover2.cpp | 1 + APMrover2/AP_Arming.cpp | 17 ++++++++++- APMrover2/AP_Arming.h | 10 +++++-- APMrover2/GCS_Mavlink.cpp | 32 ++++++++++++++++++++ APMrover2/Parameters.cpp | 7 ++++- APMrover2/Parameters.h | 3 ++ APMrover2/Rover.h | 8 ++++- APMrover2/commands_logic.cpp | 11 +++++++ APMrover2/defines.h | 5 +++- APMrover2/failsafe.cpp | 2 +- APMrover2/fence.cpp | 58 ++++++++++++++++++++++++++++++++++++ APMrover2/system.cpp | 5 ++++ APMrover2/wscript | 1 + 13 files changed, 153 insertions(+), 7 deletions(-) create mode 100644 APMrover2/fence.cpp diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index a8bbc60e6d..1c2347c7c5 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -71,6 +71,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(mount_update, 50, 600), SCHED_TASK(update_trigger, 50, 600), SCHED_TASK(gcs_failsafe_check, 10, 600), + SCHED_TASK(fence_check, 10, 100), SCHED_TASK(compass_accumulate, 50, 900), SCHED_TASK(smart_rtl_update, 3, 100), SCHED_TASK(update_notify, 50, 300), diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp index a57a5430bf..26598831c8 100644 --- a/APMrover2/AP_Arming.cpp +++ b/APMrover2/AP_Arming.cpp @@ -72,5 +72,20 @@ bool AP_Arming_Rover::gps_checks(bool display_failure) bool AP_Arming_Rover::pre_arm_checks(bool report) { - return rover.g2.motors.pre_arm_check(report) & AP_Arming::pre_arm_checks(report); + return (AP_Arming::pre_arm_checks(report) + & rover.g2.motors.pre_arm_check(report) + & fence_checks(report)); +} + +bool AP_Arming_Rover::fence_checks(bool report) +{ + // check fence is initialised + const char *fail_msg = nullptr; + if (!_fence.pre_arm_check(fail_msg)) { + if (report && fail_msg != nullptr) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Fence : %s", fail_msg); + } + return false; + } + return true; } diff --git a/APMrover2/AP_Arming.h b/APMrover2/AP_Arming.h index e558a4379a..6ce91b935b 100644 --- a/APMrover2/AP_Arming.h +++ b/APMrover2/AP_Arming.h @@ -1,6 +1,7 @@ #pragma once #include +#include /* a rover-specific arming class @@ -9,8 +10,9 @@ class AP_Arming_Rover : public AP_Arming { public: AP_Arming_Rover(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &compass, - const AP_BattMonitor &battery) - : AP_Arming(ahrs_ref, baro, compass, battery) + const AP_BattMonitor &battery, const AC_Fence &fence) + : AP_Arming(ahrs_ref, baro, compass, battery), + _fence(fence) { } @@ -24,4 +26,8 @@ public: protected: enum HomeState home_status() const override; + bool fence_checks(bool report); + +private: + const AC_Fence& _fence; }; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index ee05629f07..7cb10079fc 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -254,6 +254,11 @@ void Rover::send_pid_tuning(mavlink_channel_t chan) } } +void Rover::send_fence_status(mavlink_channel_t chan) +{ + fence_send_mavlink_status(chan); +} + void Rover::send_wheel_encoder(mavlink_channel_t chan) { // send wheel encoder data using rpm message @@ -390,6 +395,11 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) #endif // MOUNT == ENABLED break; + case MSG_FENCE_STATUS: + CHECK_PAYLOAD_SIZE(FENCE_STATUS); + rover.send_fence_status(chan); + break; + case MSG_VIBRATION: CHECK_PAYLOAD_SIZE(VIBRATION); send_vibration(rover.ins); @@ -563,6 +573,7 @@ GCS_MAVLINK_Rover::data_stream_send(void) send_message(MSG_GPS2_RAW); send_message(MSG_GPS2_RTK); send_message(MSG_NAV_CONTROLLER_OUTPUT); + send_message(MSG_FENCE_STATUS); } if (gcs().out_of_time()) { @@ -883,6 +894,21 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_DO_FENCE_ENABLE: + result = MAV_RESULT_ACCEPTED; + switch ((uint16_t)packet.param1) { + case 0: + rover.g2.fence.enable(false); + break; + case 1: + rover.g2.fence.enable(true); + break; + default: + result = MAV_RESULT_FAILED; + break; + } + break; + case MAV_CMD_GET_HOME_POSITION: if (rover.home_is_set != HOME_UNSET) { send_home(rover.ahrs.get_home()); @@ -1352,6 +1378,12 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; } + // send or receive fence points with GCS + case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + rover.g2.fence.handle_msg(*this, msg); + break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.rangefinder.handle_msg(msg); break; diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 2363dbc1af..8958a41ac1 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -542,6 +542,10 @@ 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), + AP_GROUPEND }; @@ -554,7 +558,8 @@ ParametersG2::ParametersG2(void) beacon(rover.serial_manager), motors(rover.ServoRelayEvents), attitude_control(rover.ahrs), - smart_rtl(rover.ahrs) + smart_rtl(rover.ahrs), + fence(rover.ahrs) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 3282fd2901..ad65bc62a6 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -329,6 +329,9 @@ public: // frame class for vehicle AP_Int8 frame_class; + + // fence library + AC_Fence fence; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index ef37ba7800..31f7c2267f 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -76,6 +76,7 @@ #include // Mode Filter from Filter library #include // RC Channel Library #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif @@ -174,7 +175,7 @@ private: #endif // Arming/Disarming management class - AP_Arming_Rover arming{ahrs, barometer, compass, battery}; + AP_Arming_Rover arming{ahrs, barometer, compass, battery, g2.fence}; AP_L1_Control L1_controller{ahrs, nullptr}; @@ -480,6 +481,10 @@ private: void afs_fs_check(void); #endif + // fence.cpp + void fence_check(); + void fence_send_mavlink_status(mavlink_channel_t chan); + // GCS_Mavlink.cpp void send_heartbeat(mavlink_channel_t chan); void send_attitude(mavlink_channel_t chan); @@ -492,6 +497,7 @@ private: void send_rangefinder(mavlink_channel_t chan); void send_pid_tuning(mavlink_channel_t chan); void send_wheel_encoder(mavlink_channel_t chan); + void send_fence_status(mavlink_channel_t chan); void gcs_data_stream_send(void); void gcs_update(void); void gcs_retry_deferred(void); diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 0096486d55..640214b325 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -117,6 +117,16 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) do_set_reverse(cmd); break; + case MAV_CMD_DO_FENCE_ENABLE: + if (cmd.p1 == 0) { //disable + g2.fence.enable(false); + gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); + } else { //enable fence + g2.fence.enable(true); + gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); + } + break; + default: // return false for unhandled commands return false; @@ -193,6 +203,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_SET_ROI: case MAV_CMD_DO_SET_REVERSE: + case MAV_CMD_DO_FENCE_ENABLE: return true; default: diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 612b1b2a0a..e3c187fd85 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -100,6 +100,8 @@ enum mode { #define MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE (1<<6) #define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE (1<<7) +// general error codes +#define ERROR_CODE_ERROR_RESOLVED 0 // Error message sub systems and error codes #define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9 #define ERROR_SUBSYSTEM_FLIGHT_MODE 10 @@ -122,7 +124,8 @@ enum mode_reason_t { MODE_REASON_FAILSAFE, MODE_REASON_MISSION_END, MODE_REASON_CRASH_FAILSAFE, - MODE_REASON_MISSION_COMMAND + MODE_REASON_MISSION_COMMAND, + MODE_REASON_FENCE_BREACH, }; // values used by the ap.ch7_opt and ap.ch8_opt flags diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index 5a5e15a876..08eaecb3f9 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -102,6 +102,6 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) void Rover::afs_fs_check(void) { // perform AFS failsafe checks - g2.afs.check(rover.last_heartbeat_ms, false, failsafe.last_valid_rc_ms); // Rover don't have fence + g2.afs.check(rover.last_heartbeat_ms, rover.g2.fence.get_breaches() != 0, failsafe.last_valid_rc_ms); } #endif diff --git a/APMrover2/fence.cpp b/APMrover2/fence.cpp new file mode 100644 index 0000000000..74709a690f --- /dev/null +++ b/APMrover2/fence.cpp @@ -0,0 +1,58 @@ +#include "Rover.h" + +// fence_check - ask fence library to check for breaches and initiate the response +void Rover::fence_check() +{ + uint8_t new_breaches; // the type of fence that has been breached + const uint8_t orig_breaches = g2.fence.get_breaches(); + + // check for a breach + new_breaches = g2.fence.check(); + + // return immediately if motors are not armed + if (!arming.is_armed()) { + return; + } + + // 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() != AC_FENCE_ACTION_REPORT_ONLY) { + // if we are within 100m of the fence, RTL + if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { + if (!set_mode(mode_rtl, MODE_REASON_FENCE_BREACH)) { + set_mode(mode_hold, MODE_REASON_FENCE_BREACH); + } + } else { + // if more than 100m outside the fence just force to HOLD + set_mode(mode_hold, MODE_REASON_FENCE_BREACH); + } + } + // log an error in the dataflash + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches); + + } else if (orig_breaches) { + // record clearing of breach + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED); + } +} + +// fence_send_mavlink_status - send fence status to ground station +void Rover::fence_send_mavlink_status(mavlink_channel_t chan) +{ + if (g2.fence.enabled()) { + // traslate fence library breach types to mavlink breach types + uint8_t mavlink_breach_type = FENCE_BREACH_NONE; + const uint8_t breaches = g2.fence.get_breaches(); + if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) { + mavlink_breach_type = FENCE_BREACH_BOUNDARY; + } + + // send status + mavlink_msg_fence_status_send(chan, + static_cast(g2.fence.get_breaches() != 0), + g2.fence.get_breach_count(), + mavlink_breach_type, + g2.fence.get_breach_time()); + } +} diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 6ca7e02806..89d7aa47c0 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -224,6 +224,11 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason) control_mode = &new_mode; + // 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(); + #if FRSKY_TELEM_ENABLED == ENABLED frsky_telemetry.update_control_mode(control_mode->mode_number()); #endif diff --git a/APMrover2/wscript b/APMrover2/wscript index c5dadbae3c..f075344c6b 100644 --- a/APMrover2/wscript +++ b/APMrover2/wscript @@ -22,6 +22,7 @@ def build(bld): 'AP_AdvancedFailsafe', 'AP_WheelEncoder', 'AP_SmartRTL', + 'AC_Fence', ], )