Plane: add avoidance_adsb framework for object avoidance

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

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

ArduPlane/Parameters.h

@@ -51,6 +51,7 @@ public:
         k_param_num_resets,
         k_param_NavEKF2,
         k_param_g2,
+        k_param_avoidance_adsb,
 
         // Misc
         //

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 <SITL/SITL.h>
@@ -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);

ArduPlane/avoidance_adsb.cpp

@@ -0,0 +1,41 @@
+
+#include <stdio.h>
+#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;
+}

ArduPlane/avoidance_adsb.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <AP_Avoidance/AP_Avoidance.h>
+
+// 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;
+};