From e0e2ed519cb7f0194ae74cb1977ebc2df2b7b7c0 Mon Sep 17 00:00:00 2001
From: KiwiHC16 <moulinb@free.fr>
Date: Sat, 1 Aug 2015 18:42:44 +0200
Subject: [PATCH] Rally: add RALLY_HOME_INC param to use Home as a Rally point

---
 libraries/AP_Rally/AP_Rally.cpp | 18 ++++++++++++++++++
 libraries/AP_Rally/AP_Rally.h   |  1 +
 2 files changed, 19 insertions(+)

diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp
index c0791630c8..d4927bc1e9 100644
--- a/libraries/AP_Rally/AP_Rally.cpp
+++ b/libraries/AP_Rally/AP_Rally.cpp
@@ -25,6 +25,10 @@ StorageAccess AP_Rally::_storage(StorageManager::StorageRally);
 #define RALLY_LIMIT_KM_DEFAULT 1.0f
 #endif
 
+#ifndef RALLY_HOME_INC_DEFAULT
+#define RALLY_HOME_INC_DEFAULT 0
+#endif
+
 const AP_Param::GroupInfo AP_Rally::var_info[] PROGMEM = {
     // @Param: TOTAL
     // @DisplayName: Rally Total
@@ -40,6 +44,15 @@ const AP_Param::GroupInfo AP_Rally::var_info[] PROGMEM = {
     // @Increment: 0.1
     AP_GROUPINFO("LIMIT_KM", 1, AP_Rally, _rally_limit_km, RALLY_LIMIT_KM_DEFAULT),
 
+    // @Param: HOME_INC
+    // @DisplayName: Rally Home Included
+    // @Description: Controls if Home has to be included as a Rally point (as a safe place) to RTL while a FS situation trigger a RTL. By default should be 0 as we don't know if Home is safe (e.g. boat launch...)
+    // @User: Standard
+    // @Units: na
+    // @Increment: na
+    // @Values: 0:Disabled,1:Enabled
+    AP_GROUPINFO("HOME_INC", 2, AP_Rally, _rally_home_inc, RALLY_HOME_INC_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -124,6 +137,11 @@ bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocati
         }
     }
 
+    // Check if we should take Home in the Rally points
+    if ( _rally_home_inc && (get_distance(current_loc, home_loc)<min_dis) ) {
+        return false; // use home position
+        }
+
     if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f) && (get_distance(current_loc, home_loc) < min_dis)) {
         return false; // use home position
     }
diff --git a/libraries/AP_Rally/AP_Rally.h b/libraries/AP_Rally/AP_Rally.h
index b82ee8c78a..ed0f4f7cf8 100644
--- a/libraries/AP_Rally/AP_Rally.h
+++ b/libraries/AP_Rally/AP_Rally.h
@@ -71,6 +71,7 @@ private:
     // parameters
     AP_Int8  _rally_point_total_count;
     AP_Float _rally_limit_km;
+    AP_Int8  _rally_home_inc;
 
     uint32_t _last_change_time_ms;
 };