Sub: Set EK2_ALT_NOISE on startup according to baro sensors detected.

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

ArduSub/motors.cpp

@@ -233,8 +233,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);

ArduSub/system.cpp

@@ -249,6 +249,11 @@ void Sub::init_ardupilot()
 		}
 		barometer.set_primary_baro(1); //Set the primary baro to external MS58XX
 
+		EKF.set_baro_alt_noise(0.1f);
+		EKF2.set_baro_alt_noise(0.1f);
+	} else { //We only have onboard baro
+		EKF.set_baro_alt_noise(10.0f);
+		EKF2.set_baro_alt_noise(10.0f);
 	}
 	// read Baro pressure at ground
 	//-----------------------------
@@ -273,8 +278,7 @@ void Sub::init_ardupilot()
     startup_INS_ground();
 
     // set landed flags
-    set_land_complete(true);
-    set_land_complete_maybe(true);
+    set_land_complete(false);
 
     // we don't want writes to the serial port to cause us to pause
     // mid-flight, so set the serial ports non-blocking once we are

libraries/AP_NavEKF/AP_NavEKF.h

@@ -256,6 +256,8 @@ public:
     // report any reason for why the backend is refusing to initialise
     const char *prearm_failure_reason(void) const;
 
+    void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -264,6 +264,8 @@ public:
     // report any reason for why the backend is refusing to initialise
     const char *prearm_failure_reason(void) const;
 
+    void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };
+
     // allow the enable flag to be set by Replay
     void set_enable(bool enable) { _enable.set(enable); }