diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 52662edabb..a3da782d2c 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -85,6 +85,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(terrain_update, 10, 200), SCHED_TASK(update_is_flying_5Hz, 5, 100), SCHED_TASK(dataflash_periodic, 50, 400), + SCHED_TASK(avoidance_adsb_update, 10, 100), SCHED_TASK(button_update, 5, 100), }; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 7191c7a484..adbf763006 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1145,6 +1145,10 @@ const AP_Param::Info Plane::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_Plane), + // @Group: Q_ // @Path: quadplane.cpp GOBJECT(quadplane, "Q_", QuadPlane), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 27362f242d..27fa704602 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -51,6 +51,7 @@ public: k_param_num_resets, k_param_NavEKF2, k_param_g2, + k_param_avoidance_adsb, // Misc // diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a01d7d899f..6bd1f97bc6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -102,6 +102,7 @@ #include "defines.h" #include "Parameters.h" +#include "avoidance_adsb.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include @@ -158,6 +159,7 @@ public: friend class QuadPlane; friend class AP_Tuning_Plane; friend class AP_AdvancedFailsafe_Plane; + friend class AP_Avoidance_Plane; Plane(void); @@ -655,6 +657,9 @@ private: AP_ADSB adsb {ahrs}; + // avoidance of adsb enabled vehicles (normally manned vheicles) + AP_Avoidance_Plane avoidance_adsb {ahrs, adsb}; + // Outback Challenge Failsafe Support AP_AdvancedFailsafe_Plane afs {mission, barometer, gps, rcmap}; @@ -1027,6 +1032,7 @@ private: void update_logging1(void); void update_logging2(void); void terrain_update(void); + void avoidance_adsb_update(void); void update_flight_mode(void); void stabilize(); void set_servos_idle(void); @@ -1101,6 +1107,10 @@ private: bool parachute_manual_release(); void accel_cal_update(void); + // support for AP_Avoidance custom flight mode, AVOID_ADSB + bool avoid_adsb_init(bool ignore_checks); + void avoid_adsb_run(); + public: void mavlink_delay_cb(); void failsafe_check(void); diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp new file mode 100644 index 0000000000..e0f3cf6388 --- /dev/null +++ b/ArduPlane/avoidance_adsb.cpp @@ -0,0 +1,41 @@ + +#include +#include "Plane.h" + +void Plane::avoidance_adsb_update(void) +{ + adsb.update(); + avoidance_adsb.update(); +} + + +MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) +{ + MAV_COLLISION_ACTION actual_action = requested_action; + + // return with action taken + return actual_action; +} + +void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action) +{ +} + +// check flight mode is avoid_adsb +bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change) +{ + // check flight mode + return (plane.control_mode == AVOID_ADSB); +} + +bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) +{ + // if we got this far we failed to set the new target + return false; +} + +bool AP_Avoidance_Plane::handle_avoidance_perpendicular(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) +{ + // if we got this far we failed to set the new target + return false; +} diff --git a/ArduPlane/avoidance_adsb.h b/ArduPlane/avoidance_adsb.h new file mode 100644 index 0000000000..ec02b21b4b --- /dev/null +++ b/ArduPlane/avoidance_adsb.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +// Provide Plane-specific implementation of avoidance. While most of +// the logic for doing the actual avoidance is present in +// AP_Avoidance, this class allows Plane to override base +// functionality - for example, not doing anything while landed. +class AP_Avoidance_Plane : public AP_Avoidance { + +public: + + AP_Avoidance_Plane(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 + FlightMode prev_control_mode = RTL; +};