From 22b4c9310139813ccea76cd5c87ed116d05e32a6 Mon Sep 17 00:00:00 2001
From: jaxxzer <jwalser90@gmail.com>
Date: Sat, 23 Jan 2016 22:20:07 -0500
Subject: [PATCH] Sub: Set EK2_ALT_NOISE on startup according to baro sensors
 detected.

---
 ArduSub/land_detector.cpp | 49 ++++++++++++++++++++++++++++++++++++++-
 ArduSub/motors.cpp        |  4 ++--
 2 files changed, 50 insertions(+), 3 deletions(-)

diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp
index d42f8925ee..ab93587b3e 100644
--- a/ArduSub/land_detector.cpp
+++ b/ArduSub/land_detector.cpp
@@ -24,7 +24,7 @@ void Sub::update_land_and_crash_detectors()
 // called at MAIN_LOOP_RATE
 void Sub::update_land_detector()
 {
-	if(barometer.num_instances() > 1 && barometer.get_altitude() > SURFACE_END_DEPTH && ap.throttle_zero) {
+	if(barometer.num_instances() > 1 && barometer.get_altitude() > LAND_END_DEPTH && ap.throttle_zero) {
 		set_land_complete(true);
 	} else {
 		set_land_complete(false);
@@ -46,3 +46,50 @@ void Sub::set_land_complete(bool b)
     }
     ap.land_complete = b;
 }
+
+// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
+//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
+//  has no effect when throttle is above hover throttle
+void Sub::update_throttle_thr_mix()
+{
+#if FRAME_CONFIG != HELI_FRAME
+    // if disarmed or landed prioritise throttle
+    if(!motors.armed() || ap.land_complete) {
+        motors.set_throttle_mix_min();
+        return;
+    }
+
+    if (mode_has_manual_throttle(control_mode)) {
+        // manual throttle
+        if(channel_throttle->control_in <= 0) {
+            motors.set_throttle_mix_min();
+        } else {
+            motors.set_throttle_mix_mid();
+        }
+    } else {
+        // autopilot controlled throttle
+
+        // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
+        const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
+        bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
+
+        // check for large external disturbance - angle error over 30 degrees
+        const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
+        bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
+
+        // check for large acceleration - falling or high turbulence
+        Vector3f accel_ef = ahrs.get_accel_ef_blended();
+        accel_ef.z += GRAVITY_MSS;
+        bool accel_moving = (accel_ef.length() > 3.0f);
+
+        // check for requested decent
+        bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
+
+        if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
+            motors.set_throttle_mix_max();
+        } else {
+            motors.set_throttle_mix_min();
+        }
+    }
+#endif
+}
diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp
index 6f6ffae606..02ef316faa 100644
--- a/ArduSub/motors.cpp
+++ b/ArduSub/motors.cpp
@@ -254,8 +254,8 @@ void Sub::init_disarm_motors()
 #endif
 
     // we are not in the air
-    set_land_complete(true);
-    set_land_complete_maybe(true);
+//    set_land_complete(true);// We will let the land detector decide this in sub
+//    set_land_complete_maybe(true);
 
     // log disarm to the dataflash
     Log_Write_Event(DATA_DISARMED);