Copter: integrate AP_OAPathPlanner

ArduCopter/APM_Config.h

@@ -13,6 +13,7 @@
 //#define PROXIMITY_ENABLED     DISABLED            // disable proximity sensors
 //#define AC_RALLY              DISABLED            // disable rally points library (must also disable terrain which relies on rally)
 //#define AC_AVOID_ENABLED      DISABLED            // disable stop-at-fence library
+//#define AC_OAPATHPLANNER_ENABLED DISABLED         // disable path planning around obstacles
 //#define AC_TERRAIN            DISABLED            // disable terrain library
 //#define PARACHUTE             DISABLED            // disable parachute release to save 1k of flash
 //#define NAV_GUIDED            DISABLED            // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands

ArduCopter/AP_Arming.cpp

@@ -55,7 +55,8 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
     return fence_checks(display_failure)
         & parameter_checks(display_failure)
         & motor_checks(display_failure)
-        & pilot_throttle_checks(display_failure) &
+        & pilot_throttle_checks(display_failure)
+        & oa_checks(display_failure) &
         AP_Arming::pre_arm_checks(display_failure);
 }
 
@@ -285,6 +286,25 @@ bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)
     return true;
 }
 
+bool AP_Arming_Copter::oa_checks(bool display_failure)
+{
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    char failure_msg[50];
+    if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) {
+        return true;
+    }
+    // display failure
+    if (strlen(failure_msg) == 0) {
+        check_failed(ARMING_CHECK_NONE, display_failure, "%s", "Check Object Avoidance");
+    } else {
+        check_failed(ARMING_CHECK_NONE, display_failure, "%s", failure_msg);
+    }
+    return false;
+#else
+    return true;
+#endif
+}
+
 bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
 {
     const RC_Channel *channels[] = {

ArduCopter/AP_Arming.h

@@ -45,6 +45,7 @@ protected:
     bool parameter_checks(bool display_failure);
     bool motor_checks(bool display_failure);
     bool pilot_throttle_checks(bool display_failure);
+    bool oa_checks(bool display_failure);
 
     void set_pre_arm_check(bool b);
 

ArduCopter/Copter.h

@@ -97,6 +97,10 @@
 #if AC_AVOID_ENABLED == ENABLED
  #include <AC_Avoidance/AC_Avoid.h>
 #endif
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+ #include <AC_WPNav/AC_WPNav_OA.h>
+ #include <AC_Avoidance/AP_OAPathPlanner.h>
+#endif
 #if SPRAYER_ENABLED == ENABLED
  # include <AC_Sprayer/AC_Sprayer.h>
 #endif

ArduCopter/GCS_Mavlink.cpp

@@ -270,12 +270,23 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
         // unused
         break;
 
-    case MSG_ADSB_VEHICLE:
+    case MSG_ADSB_VEHICLE: {
 #if ADSB_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
         copter.adsb.send_adsb_vehicle(chan);
+#endif
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+        AP_OADatabase *oadb = AP_OADatabase::get_singleton();
+        if (oadb != nullptr) {
+            CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
+            uint16_t interval_ms = 0;
+            if (get_ap_message_interval(id, interval_ms)) {
+                oadb->send_adsb_vehicle(chan, interval_ms);
+            }
+        }
 #endif
         break;
+    }
 
     default:
         return GCS_MAVLINK::try_send_message(id);

ArduCopter/Parameters.cpp

@@ -926,6 +926,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0),
 
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    // @Group: OA_
+    // @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp
+    AP_SUBGROUPINFO(oa, "OA_", 33, ParametersG2, AP_OAPathPlanner),
+#endif
+
     AP_GROUPEND
 };
 

ArduCopter/Parameters.h

@@ -596,6 +596,11 @@ public:
 
     AP_Float tuning_min;
     AP_Float tuning_max;
+
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    // object avoidance path planning
+    AP_OAPathPlanner oa;
+#endif
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/config.h

@@ -677,6 +677,10 @@
  #define AC_AVOID_ENABLED   ENABLED
 #endif
 
+#ifndef AC_OAPATHPLANNER_ENABLED
+ #define AC_OAPATHPLANNER_ENABLED   !HAL_MINIMIZE_FEATURES
+#endif
+
 #if AC_AVOID_ENABLED && !PROXIMITY_ENABLED
   #error AC_Avoidance relies on PROXIMITY_ENABLED which is disabled
 #endif

ArduCopter/mode.h

@@ -36,10 +36,12 @@ public:
     virtual bool landing_gear_should_be_deployed() const { return false; }
     virtual bool is_landing() const { return false; }
 
+    // functions for reporting to GCS
     virtual bool get_wp(Location &loc) { return false; };
     virtual int32_t wp_bearing() const { return 0; }
     virtual uint32_t wp_distance() const { return 0; }
     virtual float crosstrack_error() const { return 0.0f;}
+
     void update_navigation();
 
     int32_t get_alt_above_ground_cm(void);
@@ -978,6 +980,9 @@ public:
     bool allows_arming(bool from_gcs) const override { return false; };
     bool is_autopilot() const override { return true; }
 
+    // for reporting to GCS
+    bool get_wp(Location &loc) override;
+
     RTLState state() { return _state; }
 
     // this should probably not be exposed
@@ -993,6 +998,7 @@ protected:
     const char *name() const override { return "RTL"; }
     const char *name4() const override { return "RTL "; }
 
+    // for reporting to GCS
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
@@ -1056,6 +1062,8 @@ protected:
     const char *name() const override { return "SMARTRTL"; }
     const char *name4() const override { return "SRTL"; }
 
+    // for reporting to GCS
+    bool get_wp(Location &loc) override;
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
@@ -1233,9 +1241,11 @@ protected:
 
     const char *name() const override { return "FOLLOW"; }
     const char *name4() const override { return "FOLL"; }
+
+    // for reporting to GCS
+    bool get_wp(Location &loc) override;
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
-    bool get_wp(Location &loc) override;
 
     uint32_t last_log_ms;   // system time of last time desired velocity was logging
 };

ArduCopter/mode_auto.cpp

@@ -607,7 +607,9 @@ bool ModeAuto::get_wp(Location& destination)
     case Auto_NavGuided:
         return copter.mode_guided.get_wp(destination);
     case Auto_WP:
-        return wp_nav->get_wp_destination(destination);
+        return wp_nav->get_oa_wp_destination(destination);
+    case Auto_RTL:
+        return copter.mode_rtl.get_wp(destination);
     default:
         return false;
     }

ArduCopter/mode_guided.cpp

@@ -209,7 +209,7 @@ bool ModeGuided::get_wp(Location& destination)
     if (guided_mode != Guided_WP) {
         return false;
     }
-    return wp_nav->get_wp_destination(destination);
+    return wp_nav->get_oa_wp_destination(destination);
 }
 
 // sets guided mode's target from a Location object

ArduCopter/mode_rtl.cpp

@@ -496,6 +496,24 @@ void ModeRTL::compute_return_target()
     rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
 }
 
+bool ModeRTL::get_wp(Location& destination)
+{
+    // provide target in states which use wp_nav
+    switch (_state) {
+    case RTL_Starting:
+    case RTL_InitialClimb:
+    case RTL_ReturnHome:
+    case RTL_LoiterAtHome:
+    case RTL_FinalDescent:
+        return wp_nav->get_oa_wp_destination(destination);
+    case RTL_Land:
+        return false;
+    }
+
+    // we should never get here but just in case
+    return false;
+}
+
 uint32_t ModeRTL::wp_distance() const
 {
     return wp_nav->get_wp_distance_to_destination();

ArduCopter/mode_smart_rtl.cpp

@@ -149,6 +149,23 @@ void ModeSmartRTL::save_position()
     copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
 }
 
+bool ModeSmartRTL::get_wp(Location& destination)
+{
+    // provide target in states which use wp_nav
+    switch (smart_rtl_state) {
+    case SmartRTL_WaitForPathCleanup:
+    case SmartRTL_PathFollow:
+    case SmartRTL_PreLandPosition:
+    case SmartRTL_Descend:
+        return wp_nav->get_wp_destination(destination);
+    case SmartRTL_Land:
+        return false;
+    }
+
+    // we should never get here but just in case
+    return false;
+}
+
 uint32_t ModeSmartRTL::wp_distance() const
 {
     return wp_nav->get_wp_distance_to_destination();

ArduCopter/system.cpp

@@ -148,6 +148,10 @@ void Copter::init_ardupilot()
     wp_nav->set_terrain(&terrain);
 #endif
 
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    g2.oa.init();
+#endif
+
     attitude_control->parameter_sanity_check();
     pos_control->set_dt(scheduler.get_loop_period_s());
 
@@ -549,7 +553,11 @@ void Copter::allocate_motors(void)
     }
     AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);
 
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+#else
     wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+#endif
     if (wp_nav == nullptr) {
         AP_HAL::panic("Unable to allocate WPNav");
     }