From 48983c38f8cbca19cdd603ab614f553c4adb53ea Mon Sep 17 00:00:00 2001
From: jaxxzer <jwalser90@gmail.com>
Date: Fri, 26 Feb 2016 21:01:10 -0500
Subject: [PATCH] Sub: Top and bottom detection working for alt hold

---
 ArduSub/control_althold.cpp         |  5 +--
 ArduSub/motors.cpp                  |  6 ++--
 ArduSub/surface_bottom_detector.cpp | 47 +++++++++++++++--------------
 3 files changed, 31 insertions(+), 27 deletions(-)

diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 772d16ecef..7aceecddcd 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -124,8 +124,9 @@ void Sub::althold_run()
         if(ap.at_bottom) {
         	pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
             pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
-        } else if(ap.at_surface) {
-        	pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
+        } else if(ap.at_surface) { // We could use the alt max parameter for this
+        	if(motors.limit.throttle_upper) // ToDo use a better condition
+        		pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
             pos_control.set_alt_target(inertial_nav.get_altitude() - 10.0f); // set target to 10 cm below surface
         } else {
         	pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp
index 024efa6585..4de10a49f0 100644
--- a/ArduSub/motors.cpp
+++ b/ArduSub/motors.cpp
@@ -175,8 +175,10 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
 
     if (ap.home_state == HOME_UNSET) {
         // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
-        ahrs.resetHeightDatum();
-        Log_Write_Event(DATA_EKF_ALT_RESET);
+        
+        // Always use absolute altitude for ROV
+		// ahrs.resetHeightDatum();
+		// Log_Write_Event(DATA_EKF_ALT_RESET);
     } else if (ap.home_state == HOME_SET_NOT_LOCKED) {
         // Reset home position if it has already been set before (but not locked)
         set_home_to_current_location();
diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp
index 82bbc7b230..3ea661d6c6 100644
--- a/ArduSub/surface_bottom_detector.cpp
+++ b/ArduSub/surface_bottom_detector.cpp
@@ -3,7 +3,7 @@
 
 #include "Sub.h"
 
-#define SURFACE_DEPTH -0.02 // 2cm depends on where the external depth sensor is mounted
+#define SURFACE_DEPTH -0.05 // 5cm depends on where the external depth sensor is mounted
 #define DIVE_DEPTH_CONSTRAINT 0.10 // 10cm if we cannot achieve this target, we are trying to dig into the bottom
 #define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
 #define SURFACE_DETECTOR_TRIGGER_SEC 1.0
@@ -52,29 +52,30 @@ void Sub::update_surface_and_bottom_detector()
 			set_bottomed(false); // If we are not trying to descend at lower throttle limit, we are not at the bottom
 		}
 
-	} else if (accel_stationary) {
-		if(motors.limit.throttle_upper) {
-			if( surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
-				surface_detector_count++;
-			} else {
-				set_surfaced(true);
-			}
-		} else if(motors.limit.throttle_lower) {
-			// landed criteria met - increment the counter and check if we've triggered
-			if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
-				bottom_detector_count++;
-			} else {
-				set_bottomed(true);
-			}
-		} else {
-			set_surfaced(false);
-			set_bottomed(false);
-		}
-	} else {
-		// we've sensed movement up or down so reset land_detector
-		set_surfaced(false);
-		set_bottomed(false);
 	}
+//	else if (accel_stationary) {
+//		if(motors.limit.throttle_upper) {
+//			if( surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
+//				surface_detector_count++;
+//			} else {
+//				set_surfaced(true);
+//			}
+//		} else if(motors.limit.throttle_lower) {
+//			// landed criteria met - increment the counter and check if we've triggered
+//			if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
+//				bottom_detector_count++;
+//			} else {
+//				set_bottomed(true);
+//			}
+//		} else {
+//			set_surfaced(false);
+//			set_bottomed(false);
+//		}
+//	} else {
+//		// we've sensed movement up or down so reset land_detector
+//		set_surfaced(false);
+//		set_bottomed(false);
+//	}
 
 
 }