Merge branch 'sf0x_paranoid' of github.com:PX4/Firmware into swissfang

4 files changed, 25 insertions, 1 deletions

diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 80ecab2ee..d7780e9e8 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -593,13 +593,19 @@ SF0X::collect()
 		/* wipe out partially read content from last cycle(s), check for dot */
 		for (unsigned i = 0; i < (lend - 2); i++) {
 			if (_linebuf[i] == '\n') {
+				/* allocate temporary buffer */
 				char buf[sizeof(_linebuf)];
+				/* copy remainder of buffer (2nd measurement) to temporary buffer */
 				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
+				/* copy temporary buffer to beginning of line buffer,
+				 * effectively overwriting a previous temporary
+				 * measurement
+				 */
 				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
 			}
 
 			/* we need a digit before the dot and a dot for a valid number */
-			if (i > 0 && _linebuf[i] == '.') {
+			if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) {
 				valid = true;
 			}
 		}
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index a57a0481a..d27bf776f 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
 
 void TECS::_detect_underspeed(void)
 {
+	if (!_detect_underspeed_enabled) {
+		_underspeed = false;
+		return;
+	}
+
 	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
 		_underspeed = true;
 
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index bcc2d90e5..36ae4ecaf 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -66,6 +66,9 @@ public:
 		_hgt_dem_prev(0.0f),
 		_TAS_dem_adj(0.0f),
 		_STEdotErrLast(0.0f),
+		_underspeed(false),
+		_detect_underspeed_enabled(true),
+		_badDescent(false),
 		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
@@ -221,6 +224,10 @@ public:
 		_speedrate_p = speedrate_p;
 	}
 
+	void set_detect_underspeed_enabled(bool enabled) {
+		_detect_underspeed_enabled = enabled;
+	}
+
 private:
 
 	struct tecs_state _tecs_state;
@@ -323,6 +330,9 @@ private:
 	// Underspeed condition
 	bool _underspeed;
 
+	// Underspeed detection enabled
+	bool _detect_underspeed_enabled;
+
 	// Bad descent condition caused by unachievable airspeed demand
 	bool _badDescent;
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 350ce6dec..522f5caca 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1380,6 +1380,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
 				limitOverride);
 	} else {
+		/* No underspeed protection in landing mode */
+		_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
+
 		/* Using tecs library */
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,