Merge branch 'fw_autoland_att_tecs_navigator_termination_controlgroups' into bottle_drop_navigator

Conflicts:
	src/drivers/px4fmu/fmu.cpp
	src/modules/dataman/dataman.c
	src/modules/dataman/dataman.h
	src/modules/mavlink/orb_listener.c
	src/modules/mavlink/waypoints.c
	src/modules/navigator/navigator_main.cpp
	src/modules/navigator/navigator_mission.cpp
	src/modules/navigator/navigator_mission.h
	src/modules/uORB/topics/mission.h

2 files changed, 16 insertions, 0 deletions

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index d089be080..5a56dce65 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -433,10 +433,18 @@ void TECS::_update_pitch(void)
 	// Apply max and min values for integrator state that will allow for no more than
 	// 5deg of saturation. This allows for some pitch variation due to gusts before the
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
+	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
+	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
 	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	if (_climbOutDem)
+	{
+		temp += _PITCHminf * gainInv;
+	}
 	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
 
+
 	// Calculate pitch demand from specific energy balance signals
 	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
 
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index efb17225d..3699d9bce 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -46,6 +46,10 @@ namespace math
 void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
     _cutoff_freq = cutoff_freq;
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return;
+    }
     float fr = sample_freq/_cutoff_freq;
     float ohm = tanf(M_PI_F/fr);
     float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 
 float LowPassFilter2p::apply(float sample)
 {
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return sample;
+    }
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {