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

Copter: avoidance_adsb implements copter avoidance using ADSB

Browse Source
This commit is contained in:
Randy Mackay 2016-07-21 21:44:09 +09:00
parent df55704875
commit d50987f976
10 changed files with 364 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/ArduCopter.cpp
View File

@ -128,6 +128,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(accel_cal_update, 10, 100),
#if ADSB_ENABLED == ENABLED
SCHED_TASK(adsb_update, 10, 100),
SCHED_TASK(avoidance_adsb_update, 10, 400),
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
SCHED_TASK(frsky_telemetry_send, 5, 75),

12
ArduCopter/Copter.h
View File

@ -114,6 +114,7 @@
// Local modules
#include "Parameters.h"
#include "avoidance_adsb.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
@ -124,6 +125,7 @@ public:
friend class GCS_MAVLINK_Copter;
friend class AP_Rally_Copter;
friend class Parameters;
friend class AP_Avoidance_Copter;
Copter(void);
@ -554,6 +556,9 @@ private:
AP_ADSB adsb {ahrs};
// avoidance of adsb enabled vehicles (normally manned vheicles)
AP_Avoidance_Copter avoidance_adsb{ahrs, adsb};
// use this to prevent recursion during sensor init
bool in_mavlink_delay;
@ -781,6 +786,7 @@ private:
void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
void adsb_update(void);
void adsb_handle_vehicle_threats(void);
void avoidance_adsb_update(void);
bool brake_init(bool ignore_checks);
void brake_run();
void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
@ -863,6 +869,12 @@ private:
void parachute_check();
void parachute_release();
void parachute_manual_release();
// support for AP_Avoidance custom flight mode, AVOID_ADSB
bool avoid_adsb_init(bool ignore_checks);
void avoid_adsb_run();
bool avoid_adsb_set_velocity(const Vector3f& velocity_neu);
void ekf_check();
bool ekf_over_threshold();
void failsafe_ekf_event();

28
ArduCopter/Parameters.cpp
View File

@ -306,42 +306,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
@ -397,42 +397,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: CH7_OPT
// @DisplayName: Channel 7 option
// @Description: Select which function if performed when CH7 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:Avoidance
// @User: Standard
GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING),
// @Param: CH8_OPT
// @DisplayName: Channel 8 option
// @Description: Select which function if performed when CH8 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:Avoidance
// @User: Standard
GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING),
// @Param: CH9_OPT
// @DisplayName: Channel 9 option
// @Description: Select which function if performed when CH9 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:Avoidance
// @User: Standard
GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING),
// @Param: CH10_OPT
// @DisplayName: Channel 10 option
// @Description: Select which function if performed when CH10 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:Avoidance
// @User: Standard
GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING),
// @Param: CH11_OPT
// @DisplayName: Channel 11 option
// @Description: Select which function if performed when CH11 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:Avoidance
// @User: Standard
GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING),
// @Param: CH12_OPT
// @DisplayName: Channel 12 option
// @Description: Select which function if performed when CH12 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:Avoidance
// @User: Standard
GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING),
@ -907,6 +907,10 @@ const AP_Param::Info Copter::var_info[] = {
// @Path: ../libraries/AP_ADSB/AP_ADSB.cpp
GOBJECT(adsb, "ADSB_", AP_ADSB),
// @Group: AVD_
// @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp
GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter),
// @Param: AUTOTUNE_AXES
// @DisplayName: Autotune axis bitmask
// @Description: 1-byte bitmap of axes to autotune

4
ArduCopter/Parameters.h
View File

@ -183,6 +183,7 @@ public:
k_param_throw_motor_start,
k_param_terrain_follow, // 94
k_param_avoid,
k_param_avoidance_adsb,
// 97: RSSI
k_param_rssi = 97,
@ -364,6 +365,9 @@ public:
k_param_DataFlash = 253, // 253 - Logging Group
// 254,255: reserved
// the k_param_* space is 9-bits in size
// 511: reserved
};
AP_Int16 format_version;

220
ArduCopter/avoidance_adsb.cpp Normal file
View File

@ -0,0 +1,220 @@
#include "Copter.h"
#include <AP_Notify/AP_Notify.h>
void Copter::avoidance_adsb_update(void)
{
avoidance_adsb.update();
}
#include <stdio.h>
MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action)
{
MAV_COLLISION_ACTION actual_action = requested_action;
bool failsafe_state_change = false;
// check for changes in failsafe
if (!copter.failsafe.adsb) {
copter.failsafe.adsb = true;
failsafe_state_change = true;
// record flight mode in case it's required for the recovery
prev_control_mode = copter.control_mode;
}
// take no action in some flight modes
if (copter.control_mode == LAND ||
copter.control_mode == THROW ||
copter.control_mode == FLIP) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
// if landed and we will take some kind of action, just disarm
if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) {
copter.init_disarm_motors();
actual_action = MAV_COLLISION_ACTION_NONE;
} else {
// take action based on requested action
switch (actual_action) {
case MAV_COLLISION_ACTION_RTL:
// attempt to switch to RTL, if this fails (i.e. flying in manual mode with bad position) do nothing
if (failsafe_state_change) {
if (!copter.set_mode(RTL, MODE_REASON_AVOIDANCE)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
}
break;
case MAV_COLLISION_ACTION_HOVER:
// attempt to switch to Loiter, if this fails (i.e. flying in manual mode with bad position) do nothing
if (failsafe_state_change) {
if (!copter.set_mode(LOITER, MODE_REASON_AVOIDANCE)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
}
break;
case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND:
// climb or descend to avoid obstacle
if (!handle_avoidance_vertical(obstacle, failsafe_state_change)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
break;
case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY:
// move horizontally to avoid obstacle
if (!handle_avoidance_horizontal(obstacle, failsafe_state_change)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
break;
case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR:
if (!handle_avoidance_perpendicular(obstacle, failsafe_state_change)) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
break;
// unsupported actions and those that require no response
case MAV_COLLISION_ACTION_NONE:
case MAV_COLLISION_ACTION_REPORT:
default:
break;
}
}
// log to dataflash
if (failsafe_state_change) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_ADSB, actual_action);
}
// return with action taken
return actual_action;
}
void AP_Avoidance_Copter::handle_recovery(uint8_t recovery_action)
{
// check we are coming out of failsafe
if (copter.failsafe.adsb) {
copter.failsafe.adsb = false;
copter.Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_ADSB, ERROR_CODE_ERROR_RESOLVED);
// restore flight mode if requested and user has not changed mode since
if (recovery_action == AP_AVOIDANCE_RECOVERY_RETURN_TO_PREVIOUS_FLIGHTMODE && copter.control_mode_reason == MODE_REASON_AVOIDANCE) {
if (!copter.set_mode(prev_control_mode, MODE_REASON_AVOIDANCE_RECOVERY)) {
// on failure RTL or LAND
if (!copter.set_mode(RTL, MODE_REASON_AVOIDANCE_RECOVERY)) {
copter.set_mode(LAND, MODE_REASON_AVOIDANCE_RECOVERY);
}
}
}
}
}
// check flight mode is avoid_adsb
bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change)
{
// ensure copter is in avoid_adsb mode
if (allow_mode_change && copter.control_mode != AVOID_ADSB) {
if (!copter.set_mode(AVOID_ADSB, MODE_REASON_AVOIDANCE)) {
// failed to set mode so exit immediately
return false;
}
}
// check flight mode
return (copter.control_mode == AVOID_ADSB);
}
bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
{
// ensure copter is in avoid_adsb mode
if (!check_flightmode(allow_mode_change)) {
return false;
}
// decide on whether we should climb or descend
bool should_climb = false;
Location my_loc;
if (_ahrs.get_position(my_loc)) {
should_climb = my_loc.alt > obstacle->_location.alt;
}
// get best vector away from obstacle
Vector3f velocity_neu;
if (should_climb) {
velocity_neu.z = copter.wp_nav.get_speed_up();
} else {
velocity_neu.z = -copter.wp_nav.get_speed_down();
// do not descend if below RTL alt
if (copter.current_loc.alt < copter.g.rtl_altitude) {
velocity_neu.z = 0.0f;
}
}
// send target velocity
copter.avoid_adsb_set_velocity(velocity_neu);
return true;
}
bool AP_Avoidance_Copter::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
{
// ensure copter is in avoid_adsb mode
if (!check_flightmode(allow_mode_change)) {
return false;
}
// get best vector away from obstacle
Vector3f velocity_neu;
if (get_vector_perpendicular(obstacle, velocity_neu)) {
// remove vertical component
velocity_neu.z = 0.0f;
// check for divide by zero
if (is_zero(velocity_neu.x) && is_zero(velocity_neu.y)) {
return false;
}
// re-normalise
velocity_neu.normalize();
// convert horizontal components to velocities
velocity_neu.x *= copter.wp_nav.get_speed_xy();
velocity_neu.y *= copter.wp_nav.get_speed_xy();
// send target velocity
copter.avoid_adsb_set_velocity(velocity_neu);
return true;
}
// if we got this far we failed to set the new target
return false;
}
bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
{
// ensure copter is in avoid_adsb mode
if (!check_flightmode(allow_mode_change)) {
return false;
}
// get best vector away from obstacle
Vector3f velocity_neu;
if (get_vector_perpendicular(obstacle, velocity_neu)) {
// convert horizontal components to velocities
velocity_neu.x *= copter.wp_nav.get_speed_xy();
velocity_neu.y *= copter.wp_nav.get_speed_xy();
// use up and down waypoint speeds
if (velocity_neu.z > 0.0f) {
velocity_neu.z *= copter.wp_nav.get_speed_up();
} else {
velocity_neu.z *= copter.wp_nav.get_speed_down();
// do not descend if below RTL alt
if (copter.current_loc.alt < copter.g.rtl_altitude) {
velocity_neu.z = 0.0f;
}
}
// send target velocity
copter.avoid_adsb_set_velocity(velocity_neu);
return true;
}
// if we got this far we failed to set the new target
return false;
}

38
ArduCopter/avoidance_adsb.h Normal file
View File

@ -0,0 +1,38 @@
#pragma once
#include <AP_Avoidance/AP_Avoidance.h>
// Provide Copter-specific implementation of avoidance. While most of
// the logic for doing the actual avoidance is present in
// AP_Avoidance, this class allows Copter to override base
// functionality - for example, not doing anything while landed.
class AP_Avoidance_Copter : public AP_Avoidance {
public:
AP_Avoidance_Copter(AP_AHRS &ahrs, class AP_ADSB &adsb) :
AP_Avoidance(ahrs, adsb) { }
protected:
// override avoidance handler
MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) override;
// override recovery handler
void handle_recovery(uint8_t recovery_action) override;
// check flight mode is avoid_adsb
bool check_flightmode(bool allow_mode_change);
// vertical avoidance handler
bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// horizontal avoidance handler
bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// perpendicular (3 dimensional) avoidance handler
bool handle_avoidance_perpendicular(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// control mode before avoidance began
control_mode_t prev_control_mode = RTL;
};

40
ArduCopter/control_avoid_adsb.cpp Normal file
View File

@ -0,0 +1,40 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_avoid.cpp - init and run calls for AP_Avoidance's AVOID flight mode
*
* This re-uses GUIDED mode functions but does not interfere with the GCS or companion computer's
* use of guided mode because the velocity requests arrive from different sources (i.e MAVLink messages
* for GCS and Companion Computers vs the AP_Avoidance_Copter class for adsb avoidance) and inputs from
* each source are only accepted and processed in the appropriate flight mode.
*/
// initialise avoid_adsb controller
bool Copter::avoid_adsb_init(const bool ignore_checks)
{
// re-use guided mode
return guided_init(ignore_checks);
}
bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu)
{
// check flight mode
if (control_mode != AVOID_ADSB) {
return false;
}
// re-use guided mode's velocity controller
guided_set_velocity(velocity_neu);
return true;
}
// runs the AVOID_ADSB controller
void Copter::avoid_adsb_run()
{
// re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
// position and velocity requests will be ignored while the vehicle is not in guided mode
guided_run();
}

12
ArduCopter/defines.h
View File

@ -67,7 +67,8 @@ enum aux_sw_func {
AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34)
AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35)
AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36)
AUXSW_THROW = 37 // change to THROW flight mode
AUXSW_THROW = 37, // change to THROW flight mode
AUXSW_AVOID_ADSB = 38, // enable AP_Avoidance library
};
// Frame types
@ -103,7 +104,8 @@ enum control_mode_t {
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18 // throw to launch mode using inertial/GPS system, no pilot input
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
};
enum mode_reason_t {
@ -120,7 +122,9 @@ enum mode_reason_t {
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
};
// Tuning enumeration
@ -351,6 +355,8 @@ enum ThrowModeState {
#define DATA_EKF_ALT_RESET 60
#define DATA_LAND_CANCELLED_BY_PILOT 61
#define DATA_EKF_YAW_RESET 62
#define DATA_AVOIDANCE_ADSB_ENABLE 63
#define DATA_AVOIDANCE_ADSB_DISABLE 64
// Centi-degrees to radians
#define DEGX100 5729.57795f

13
ArduCopter/flight_mode.cpp
View File

@ -103,6 +103,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
success = throw_init(ignore_checks);
break;
case AVOID_ADSB:
success = avoid_adsb_init(ignore_checks);
break;
default:
success = false;
break;
@ -226,6 +230,10 @@ void Copter::update_flight_mode()
throw_run();
break;
case AVOID_ADSB:
avoid_adsb_run();
break;
default:
break;
}
@ -294,6 +302,7 @@ bool Copter::mode_requires_GPS(control_mode_t mode) {
case DRIFT:
case POSHOLD:
case BRAKE:
case AVOID_ADSB:
case THROW:
return true;
default:
@ -332,6 +341,7 @@ void Copter::notify_flight_mode(control_mode_t mode) {
case GUIDED:
case RTL:
case CIRCLE:
case AVOID_ADSB:
case LAND:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
@ -397,6 +407,9 @@ void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case THROW:
port->print("THROW");
break;
case AVOID_ADSB:
port->print("AVOID_ADSB");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;

11
ArduCopter/switches.cpp
View File

@ -601,6 +601,17 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
}
break;
case AUXSW_AVOID_ADSB:
// enable or disable AP_Avoidance
if (ch_flag == AUX_SWITCH_HIGH) {
avoidance_adsb.enable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
}else{
avoidance_adsb.disable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
break;
}
}