Rover: integrate OAPathPlanner

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;
+}

APMrover2/AP_Arming.h

@@ -27,6 +27,7 @@ public:
     void update_soft_armed();
 
 protected:
+    bool oa_check(bool report);
 
 private:
 

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

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[];

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>

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;

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;

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;

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());