From c233c3aff35feab7ec5bb501f8d71dabfa3013bb Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 10 May 2019 14:59:52 +0900
Subject: [PATCH] Rover: integrate OAPathPlanner

---
 APMrover2/AP_Arming.cpp   | 20 +++++++++++++++++++-
 APMrover2/AP_Arming.h     |  1 +
 APMrover2/Parameters.cpp  |  4 ++++
 APMrover2/Parameters.h    |  3 +++
 APMrover2/Rover.h         |  1 +
 APMrover2/mode_auto.cpp   |  2 +-
 APMrover2/mode_guided.cpp |  2 +-
 APMrover2/mode_rtl.cpp    |  2 +-
 APMrover2/system.cpp      |  3 +++
 9 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp
index 2985b9dfcf..05919a97e3 100644
--- a/APMrover2/AP_Arming.cpp
+++ b/APMrover2/AP_Arming.cpp
@@ -92,7 +92,8 @@ bool AP_Arming_Rover::pre_arm_checks(bool report)
 
     return (AP_Arming::pre_arm_checks(report)
             & rover.g2.motors.pre_arm_check(report)
-            & fence_checks(report));
+            & fence_checks(report)
+            & oa_check(report));
 }
 
 bool AP_Arming_Rover::arm_checks(AP_Arming::Method method)
@@ -166,3 +167,20 @@ bool AP_Arming_Rover::disarm(void)
 
     return true;
 }
+
+// check object avoidance has initialised correctly
+bool AP_Arming_Rover::oa_check(bool report)
+{
+    char failure_msg[50];
+    if (rover.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, report, "Check Object Avoidance");
+    } else {
+        check_failed(ARMING_CHECK_NONE, report, failure_msg);
+    }
+    return false;
+}
diff --git a/APMrover2/AP_Arming.h b/APMrover2/AP_Arming.h
index 52306a3374..94e5c9bf45 100644
--- a/APMrover2/AP_Arming.h
+++ b/APMrover2/AP_Arming.h
@@ -27,6 +27,7 @@ public:
     void update_soft_armed();
 
 protected:
+    bool oa_check(bool report);
 
 private:
 
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 89dbbe0994..396aad133f 100644
--- a/APMrover2/Parameters.cpp
+++ b/APMrover2/Parameters.cpp
@@ -633,6 +633,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: sailboat.cpp
     AP_SUBGROUPINFO(sailboat, "SAIL_", 44, ParametersG2, Sailboat),
 
+    // @Group: OA_
+    // @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp
+    AP_SUBGROUPINFO(oa, "OA_", 45, ParametersG2, AP_OAPathPlanner),
+
     AP_GROUPEND
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index d3d471dd78..787f357ed3 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -392,6 +392,9 @@ public:
 
     // Sailboat functions
     Sailboat sailboat;
+
+    // object avoidance path planning
+    AP_OAPathPlanner oa;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 0b6cba744b..2f145f42b1 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -77,6 +77,7 @@
 #include <AC_Fence/AC_Fence.h>
 #include <AP_Proximity/AP_Proximity.h>
 #include <AC_Avoidance/AC_Avoid.h>
+#include <AC_Avoidance/AP_OAPathPlanner.h>
 #include <AP_Follow/AP_Follow.h>
 #include <AP_OSD/AP_OSD.h>
 #include <AP_WindVane/AP_WindVane.h>
diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp
index fdbca4b923..2cb157665a 100644
--- a/APMrover2/mode_auto.cpp
+++ b/APMrover2/mode_auto.cpp
@@ -137,7 +137,7 @@ bool ModeAuto::get_desired_location(Location& destination) const
     switch (_submode) {
     case Auto_WP:
         if (g2.wp_nav.is_destination_valid()) {
-            destination = g2.wp_nav.get_destination();
+            destination = g2.wp_nav.get_oa_destination();
             return true;
         }
         return false;
diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp
index 098f016b8b..b766d97ef4 100644
--- a/APMrover2/mode_guided.cpp
+++ b/APMrover2/mode_guided.cpp
@@ -150,7 +150,7 @@ bool ModeGuided::get_desired_location(Location& destination) const
     switch (_guided_mode) {
     case Guided_WP:
         if (g2.wp_nav.is_destination_valid()) {
-            destination = g2.wp_nav.get_destination();
+            destination = g2.wp_nav.get_oa_destination();
             return true;
         }
         return false;
diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp
index 126396b8a6..3f36610f91 100644
--- a/APMrover2/mode_rtl.cpp
+++ b/APMrover2/mode_rtl.cpp
@@ -62,7 +62,7 @@ void ModeRTL::update()
 bool ModeRTL::get_desired_location(Location& destination) const
 {
     if (g2.wp_nav.is_destination_valid()) {
-        destination = g2.wp_nav.get_destination();
+        destination = g2.wp_nav.get_oa_destination();
         return true;
     }
     return false;
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index a188a26805..844e781040 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -144,6 +144,9 @@ void Rover::init_ardupilot()
     // initialize SmartRTL
     g2.smart_rtl.init();
 
+    // initialise object avoidance
+    g2.oa.init();
+
     startup_ground();
 
     Mode *initial_mode = mode_from_mode_num((enum Mode::Number)g.initial_mode.get());