From 709c874d8bfe89920c00fe111e74a9a2a6638ffb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jun 2019 19:47:32 +0900 Subject: [PATCH] Copter: integrate AP_OAPathPlanner --- ArduCopter/APM_Config.h | 1 + ArduCopter/AP_Arming.cpp | 22 +++++++++++++++++++++- ArduCopter/AP_Arming.h | 1 + ArduCopter/Copter.h | 4 ++++ ArduCopter/GCS_Mavlink.cpp | 13 ++++++++++++- ArduCopter/Parameters.cpp | 6 ++++++ ArduCopter/Parameters.h | 5 +++++ ArduCopter/config.h | 4 ++++ ArduCopter/mode.h | 12 +++++++++++- ArduCopter/mode_auto.cpp | 4 +++- ArduCopter/mode_guided.cpp | 2 +- ArduCopter/mode_rtl.cpp | 18 ++++++++++++++++++ ArduCopter/mode_smart_rtl.cpp | 17 +++++++++++++++++ ArduCopter/system.cpp | 8 ++++++++ 14 files changed, 112 insertions(+), 5 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 1570888c30..5608af91ae 100644 --- a/ArduCopter/APM_Config.h +++ b/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 diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 22f9cf82fd..959a2dc712 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/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[] = { diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index a5909f83db..6cc30cebc9 100644 --- a/ArduCopter/AP_Arming.h +++ b/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); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 681931c380..c13531ba9e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -97,6 +97,10 @@ #if AC_AVOID_ENABLED == ENABLED #include #endif +#if AC_OAPATHPLANNER_ENABLED == ENABLED + #include + #include +#endif #if SPRAYER_ENABLED == ENABLED # include #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 7ac23b070b..591a6d1706 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/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); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index bdb0551dae..8a2b55b7f1 100644 --- a/ArduCopter/Parameters.cpp +++ b/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 }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index a0dc9b26a9..26e197db02 100644 --- a/ArduCopter/Parameters.h +++ b/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[]; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 80ad2e2559..b353668127 100644 --- a/ArduCopter/config.h +++ b/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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 65383503b4..93e631c73b 100644 --- a/ArduCopter/mode.h +++ b/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 }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 81598dc207..f8df68e3ae 100644 --- a/ArduCopter/mode_auto.cpp +++ b/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; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f3ad2d44ef..e9ce6180c9 100644 --- a/ArduCopter/mode_guided.cpp +++ b/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 diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index dc5f603d12..3b2e60472f 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/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(); diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 6bb4867a27..610105b6ed 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/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(); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index b430bbedcd..c76fc58f4e 100755 --- a/ArduCopter/system.cpp +++ b/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"); }