From d50987f976efa999a8dabff1b7b8fcea6f4553ca Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 21 Jul 2016 21:44:09 +0900 Subject: [PATCH] Copter: avoidance_adsb implements copter avoidance using ADSB --- ArduCopter/ArduCopter.cpp | 1 + ArduCopter/Copter.h | 12 ++ ArduCopter/Parameters.cpp | 28 ++-- ArduCopter/Parameters.h | 4 + ArduCopter/avoidance_adsb.cpp | 220 ++++++++++++++++++++++++++++++ ArduCopter/avoidance_adsb.h | 38 ++++++ ArduCopter/control_avoid_adsb.cpp | 40 ++++++ ArduCopter/defines.h | 12 +- ArduCopter/flight_mode.cpp | 13 ++ ArduCopter/switches.cpp | 11 ++ 10 files changed, 364 insertions(+), 15 deletions(-) create mode 100644 ArduCopter/avoidance_adsb.cpp create mode 100644 ArduCopter/avoidance_adsb.h create mode 100644 ArduCopter/control_avoid_adsb.cpp diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 9033bfb969..0a990d3bd2 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -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), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 280fb8383e..6468c6e457 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -114,6 +114,7 @@ // Local modules #include "Parameters.h" +#include "avoidance_adsb.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include @@ -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(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 6e284651b0..577bc9a02e 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4f95c6e034..2aef30dbf1 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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; diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp new file mode 100644 index 0000000000..0ece914eda --- /dev/null +++ b/ArduCopter/avoidance_adsb.cpp @@ -0,0 +1,220 @@ +#include "Copter.h" +#include + +void Copter::avoidance_adsb_update(void) +{ + avoidance_adsb.update(); +} + +#include + +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; +} diff --git a/ArduCopter/avoidance_adsb.h b/ArduCopter/avoidance_adsb.h new file mode 100644 index 0000000000..f4e0ff40d2 --- /dev/null +++ b/ArduCopter/avoidance_adsb.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +// 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; +}; diff --git a/ArduCopter/control_avoid_adsb.cpp b/ArduCopter/control_avoid_adsb.cpp new file mode 100644 index 0000000000..0fccbe398b --- /dev/null +++ b/ArduCopter/control_avoid_adsb.cpp @@ -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(); +} diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f5e201ace8..7e38111768 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 7aa5a5ca34..2c3e090434 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -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; diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index f235ef485d..f57228033b 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -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; } }