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

Rover: add fence support

Browse Source
This commit is contained in:
khancyr 2017-08-16 12:02:56 +02:00 committed by Randy Mackay
parent ae4ded86a8
commit c3fb985ec5
13 changed files with 153 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
APMrover2/APMrover2.cpp
View File

@ -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),

17
APMrover2/AP_Arming.cpp
View File

@ -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;
}

10
APMrover2/AP_Arming.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <AP_Arming/AP_Arming.h>
#include <AC_Fence/AC_Fence.h>
/*
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;
};

32
APMrover2/GCS_Mavlink.cpp
View File

@ -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;

7
APMrover2/Parameters.cpp
View File

@ -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);
}

3
APMrover2/Parameters.h
View File

@ -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[];

8
APMrover2/Rover.h
View File

@ -76,6 +76,7 @@
#include <Filter/ModeFilter.h> // Mode Filter from Filter library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <StorageManager/StorageManager.h>
#include <AC_Fence/AC_Fence.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#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);

11
APMrover2/commands_logic.cpp
View File

@ -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:

5
APMrover2/defines.h
View File

@ -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

2
APMrover2/failsafe.cpp
View File

@ -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

58
APMrover2/fence.cpp Normal file
View File

@ -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<int8_t>(g2.fence.get_breaches() != 0),
g2.fence.get_breach_count(),
mavlink_breach_type,
g2.fence.get_breach_time());
}
}

5
APMrover2/system.cpp
View File

@ -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

1
APMrover2/wscript
View File

@ -22,6 +22,7 @@ def build(bld):
'AP_AdvancedFailsafe',
'AP_WheelEncoder',
'AP_SmartRTL',
'AC_Fence',
],
)