Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: Add support for AC_Fence 

ArduPlane: Remove some geofence logic from ArduPlane

ArduPlane: Refactor action and mode return implementation

ArduPlane: Use polyfence to find the return point.
This commit is contained in:
James O'Shannessy 2020-09-10 16:31:18 +10:00 committed by Peter Barker
parent 9b53d505ea
commit 17fb585bf1
14 changed files with 274 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduPlane/ArduPlane.cpp
View File

@ -58,6 +58,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300),
SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150),
SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300),
#if AC_FENCE == ENABLED
SCHED_TASK_CLASS(AC_Fence, &plane.fence, update, 10, 100),
#endif
SCHED_TASK(read_rangefinder, 50, 100),
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100),
SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50),
@ -66,6 +69,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50),
#endif
SCHED_TASK(one_second_loop, 1, 400),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(check_long_failsafe, 3, 400),
SCHED_TASK(rpm_update, 10, 100),
#if AP_AIRSPEED_AUTOCAL_ENABLE
@ -243,7 +247,12 @@ void Plane::update_logging2(void)
void Plane::afs_fs_check(void)
{
// perform AFS failsafe checks
afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
#if AC_FENCE == ENABLED
const bool fence_breached = fence.get_breaches() != 0;
#else
const bool fence_breached = false;
#endif
afs.check(failsafe.last_heartbeat_ms, fence_breached, failsafe.AFS_last_valid_rc_ms);
}
#endif
@ -299,6 +308,13 @@ void Plane::one_second_loop()
}
}
void Plane::three_hz_loop()
{
#if AC_FENCE == ENABLED
fence_check();
#endif
}
void Plane::compass_save()
{
if (AP::compass().enabled() &&
@ -391,8 +407,10 @@ void Plane::update_GPS_10Hz(void)
}
}
#if GEOFENCE_ENABLED == ENABLED
// see if we've breached the geo-fence
geofence_check(false);
#endif
// update wind estimate
ahrs.estimate_wind();
@ -478,7 +496,9 @@ void Plane::update_alt()
#if PARACHUTE == ENABLED
parachute.set_sink_rate(auto_state.sink_rate);
#endif
#if GEOFENCE_ENABLED == ENABLED
geofence_check(true);
#endif
update_flight_stage();

5
ArduPlane/Attitude.cpp
View File

@ -58,7 +58,12 @@ bool Plane::stick_mixing_enabled(void)
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
g.stick_mixing != STICK_MIXING_VTOL_YAW &&
#if GEOFENCE_ENABLED == ENABLED
geofence_stickmixing() &&
#endif
#if AC_FENCE == ENABLED
fence_stickmixing() &&
#endif
failsafe.state == FAILSAFE_NONE &&
!rc_failsafe_active()) {
// we're in an auto mode, and haven't triggered failsafe

7
ArduPlane/GCS_Mavlink.cpp
View File

@ -415,12 +415,12 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
#endif
break;
case MSG_FENCE_STATUS:
#if GEOFENCE_ENABLED == ENABLED
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
plane.send_fence_status(chan);
#endif
break;
#endif
case MSG_TERRAIN:
#if AP_TERRAIN_AVAILABLE
@ -1050,7 +1050,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_FAILED;
#if GEOFENCE_ENABLED == ENABLED
case MAV_CMD_DO_FENCE_ENABLE:
if (!plane.geofence_present()) {
gcs().send_text(MAV_SEVERITY_NOTICE,"Fence not configured");
return MAV_RESULT_FAILED;
@ -1076,6 +1078,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
break;
}
return MAV_RESULT_FAILED;
#endif
case MAV_CMD_DO_SET_HOME: {
// param1 : use current (1=use current location, 0=use specified location)

5
ArduPlane/GCS_Plane.cpp
View File

@ -20,9 +20,12 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (plane.geofence_present()) {
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
@ -32,9 +35,11 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#if GEOFENCE_ENABLED == ENABLED
if (plane.geofence_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |

6
ArduPlane/Parameters.cpp
View File

@ -1073,6 +1073,12 @@ 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 AP_AHRS_NAVEKF_AVAILABLE
#if HAL_NAVEKF2_AVAILABLE
// @Group: EK2_

1
ArduPlane/Parameters.h
View File

@ -350,6 +350,7 @@ public:
k_param_gcs4, // stream rates
k_param_gcs5, // stream rates
k_param_gcs6, // stream rates
k_param_fence, // vehicle fence
};
AP_Int16 format_version;

19
ArduPlane/Plane.h
View File

@ -98,6 +98,10 @@
#include "afs_plane.h"
#endif
#if AC_FENCE == ENABLED
#include <AC_Fence/AC_Fence.h>
#endif
// Local modules
#include "defines.h"
#include "mode.h"
@ -256,7 +260,11 @@ private:
#if OSD_ENABLED || OSD_PARAM_ENABLED
AP_OSD osd;
#endif
#if AC_FENCE == ENABLED
AC_Fence fence;
#endif
ModeCircle mode_circle;
ModeStabilize mode_stabilize;
ModeTraining mode_training;
@ -918,6 +926,13 @@ private:
void failsafe_long_off_event(ModeReason reason);
void handle_battery_failsafe(const char* type_str, const int8_t action);
#if AC_FENCE == ENABLED
// fence.cpp
void fence_check();
bool fence_stickmixing() const;
#endif
#if GEOFENCE_ENABLED == ENABLED
// geofence.cpp
uint8_t max_fencepoints(void) const;
Vector2l get_fence_point_with_index(uint8_t i) const;
@ -937,6 +952,7 @@ private:
bool geofence_breached(void);
void geofence_disable_and_send_error_msg(const char *errorMsg);
void disable_fence_for_landing(void);
#endif
// ArduPlane.cpp
void disarm_if_autoland_complete();
@ -955,6 +971,7 @@ private:
void afs_fs_check(void);
#endif
void one_second_loop(void);
void three_hz_loop(void);
#if AP_AIRSPEED_AUTOCAL_ENABLE
void airspeed_ratio_update(void);
#endif

44
ArduPlane/commands_logic.cpp
View File

@ -131,6 +131,18 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_DO_FENCE_ENABLE:
#if AC_FENCE == ENABLED
if (cmd.p1 == 0) { // disable fence
plane.fence.enable(false);
gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled");
} else if (cmd.p1 == 1) { // enable fence
plane.fence.enable(true);
gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled");
} else if (cmd.p1 == 2) { // disable fence floor only
plane.fence.disable_floor();
gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled");
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (cmd.p1 != 2) {
if (!geofence_set_enabled((bool) cmd.p1)) {
@ -385,7 +397,37 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
disable_fence_for_landing();
#if AC_FENCE == ENABLED
switch(fence.auto_enabled()) {
case AC_Fence::AutoEnable::ALWAYS_ENABLED:
fence.enable(false);
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
break;
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
fence.disable_floor();
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
break;
default:
// fence does not auto-disable in other landing conditions
break;
}
#endif
#if GEOFENCE_ENABLED == ENABLED
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 == FenceAutoEnable::AutoDisableFloorOnly) {
if (!geofence_set_floor_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
}
void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)

14
ArduPlane/config.h
View File

@ -246,6 +246,20 @@
# define SCALING_SPEED 15.0
#endif
#if 1
// Use AC_FENCE
#define AC_FENCE ENABLED
#define GEOFENCE_ENABLED DISABLED
#else
// Use GEOFENCE
#define AC_FENCE DISABLED
#define GEOFENCE_ENABLED ENABLED
#endif
#ifndef AC_FENCE
# define AC_FENCE DISABLED
#endif
// use this to disable geo-fencing
#ifndef GEOFENCE_ENABLED
# define GEOFENCE_ENABLED ENABLED

129
ArduPlane/fence.cpp Normal file
View File

@ -0,0 +1,129 @@
#include "Plane.h"
// Code to integrate AC_Fence library with main ArduPlane code
#if AC_FENCE == ENABLED
// fence_check - ask fence library to check for breaches and initiate the response
void Plane::fence_check()
{
const uint8_t orig_breaches = fence.get_breaches();
// check for new breaches; new_breaches is bitmask of fence types breached
const uint8_t new_breaches = fence.check();
if (!fence.enabled()) {
// Switch back to the chosen control mode if still in
// GUIDED to the return point
switch(fence.get_action()) {
case AC_FENCE_ACTION_GUIDED:
case AC_FENCE_ACTION_GUIDED_THROTTLE_PASS:
case AC_FENCE_ACTION_RTL_AND_LAND:
if (plane.control_mode_reason == ModeReason::FENCE_BREACHED &&
(control_mode == &mode_guided || control_mode == &mode_avoidADSB)) {
set_mode(*previous_mode, ModeReason::FENCE_RETURN_PREVIOUS_MODE);
}
break;
default:
// No returning to a previous mode, unless our action allows it
break;
}
}
// we still don't do anything when disarmed, but we do check for fence breaches.
// fence pre-arm check actually checks if any fence has been breached
// that's not ever going to be true if we don't call check on AP_Fence while disarmed
if (!arming.is_armed()) {
return;
}
// Never trigger a fence breach in the final stage of landing
if (landing.is_expecting_impact()) {
return;
}
if (new_breaches) {
// if the user wants some kind of response and motors are armed
const uint8_t fence_act = fence.get_action();
switch (fence_act) {
case AC_FENCE_ACTION_REPORT_ONLY:
break;
case AC_FENCE_ACTION_GUIDED:
case AC_FENCE_ACTION_GUIDED_THROTTLE_PASS:
case AC_FENCE_ACTION_RTL_AND_LAND:
// make sure we don't auto trim the surfaces on this mode change
int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0);
if (fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
set_mode(mode_rtl, ModeReason::FENCE_BREACHED);
} else {
set_mode(mode_guided, ModeReason::FENCE_BREACHED);
}
g.auto_trim.set(saved_auto_trim);
if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
} else {
//return to fence return point, not a rally point
guided_WP_loc = {};
if (fence.get_return_altitude() > 0) {
// fly to the return point using _retalt
guided_WP_loc.alt = home.alt + 100.0f * fence.get_return_altitude();
} else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) {
// invalid min/max, use RTL_altitude
guided_WP_loc.alt = home.alt + g.RTL_altitude_cm;
} else {
// fly to the return point, with an altitude half way between
// min and max
guided_WP_loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2;
}
Vector2l return_point;
if(fence.polyfence().get_return_point(return_point)) {
guided_WP_loc.lat = return_point[0];
guided_WP_loc.lng = return_point[1];
} else {
// should. not. happen.
guided_WP_loc.lat = current_loc.lat;
guided_WP_loc.lng = current_loc.lng;
}
}
//! TODO: Update setting of guided location
/*
fence_state->guided_lat = guided_WP_loc.lat;
fence_state->guided_lng = guided_WP_loc.lng;
*/
if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) {
setup_terrain_target_alt(guided_WP_loc);
set_guided_WP();
}
if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) {
guided_throttle_passthru = true;
}
break;
}
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
} else if (orig_breaches) {
// record clearing of breach
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
}
}
bool Plane::fence_stickmixing(void) const
{
if (fence.enabled() &&
fence.get_breaches() &&
(control_mode == &mode_guided || control_mode == &mode_avoidADSB))
{
// don't mix in user input
return false;
}
// normal mixing rules
return true;
}
#endif

4
ArduPlane/geofence.cpp
View File

@ -53,6 +53,7 @@ Vector2l Plane::get_fence_point_with_index(uint8_t i) const
{
if (i > (uint8_t)g.fence_total || i >= max_fencepoints()) {
return Vector2l(0,0);
}
// read fence point
@ -532,7 +533,7 @@ void Plane::disable_fence_for_landing(void)
#else // GEOFENCE_ENABLED
#if 0
void Plane::geofence_check(bool altitude_check_only) {
}
bool Plane::geofence_stickmixing(void) {
@ -558,6 +559,7 @@ bool Plane::geofence_breached(void)
{
return false;
}
#endif
void Plane::disable_fence_for_landing(void)
{

9
ArduPlane/radio.cpp
View File

@ -188,7 +188,14 @@ void Plane::read_radio()
airspeed_nudge_cm = 0;
throttle_nudge = 0;
if (g.throttle_nudge && channel_throttle->get_control_in() > 50 && geofence_stickmixing()) {
if (g.throttle_nudge && channel_throttle->get_control_in() > 50
#if GEOFENCE_ENABLED == ENABLED
&& geofence_stickmixing()
#endif
#if AC_FENCE == ENABLED
&& fence_stickmixing()
#endif
) {
float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;

5
ArduPlane/system.cpp
View File

@ -154,6 +154,11 @@ void Plane::init_ardupilot()
#if GRIPPER_ENABLED == ENABLED
g2.gripper.init();
#endif
// init fence
#if AC_FENCE == ENABLED
fence.init();
#endif
}
//********************************************************************************

11
ArduPlane/takeoff.cpp
View File

@ -275,6 +275,17 @@ return_zero:
*/
void Plane::complete_auto_takeoff(void)
{
#if AC_FENCE == ENABLED
switch(fence.auto_enabled()) {
case AC_Fence::AutoEnable::ALWAYS_ENABLED:
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
fence.enable(true);
break;
default:
// fence does not auto-enable in other takeoff conditions
break;
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable != FenceAutoEnable::OFF) {
if (! geofence_set_enabled(true)) {