fw_pos_control_l1 skip wind > max airspeed logic if airspeed invalid

- cleanup airspeed validity check logic
PX4 · Jan 12, 2019 · 45d7165 · 45d7165
1 parent 2e3fa30
commit 45d7165
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 29 deletions.
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -374,35 +374,36 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 void
 FixedwingPositionControl::airspeed_poll()
 {
-	if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
+	bool airspeed_valid = _airspeed_valid;
 
-		const airspeed_s &airspeed = _sub_airspeed.get();
+	if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
 
-		_airspeed_valid = PX4_ISFINITE(airspeed.indicated_airspeed_m_s)
-				  && PX4_ISFINITE(airspeed.true_airspeed_m_s)
-				  && (airspeed.indicated_airspeed_m_s > 0.0f);
+		const airspeed_s &as = _sub_airspeed.get();
 
-		_airspeed_last_received = hrt_absolute_time();
-		_airspeed = airspeed.indicated_airspeed_m_s;
+		if (PX4_ISFINITE(as.indicated_airspeed_m_s)
+		    && PX4_ISFINITE(as.true_airspeed_m_s)
+		    && (as.indicated_airspeed_m_s > 0.0f)) {
 
-		if (_airspeed_valid
-		    && airspeed.true_airspeed_m_s > airspeed.indicated_airspeed_m_s) {
+			airspeed_valid = true;
 
-			_eas2tas = max(airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, 1.0f);
+			_airspeed_last_valid = as.timestamp;
+			_airspeed = as.indicated_airspeed_m_s;
 
-		} else {
-			_eas2tas = 1.0f;
+			_eas2tas = constrain(as.true_airspeed_m_s / as.indicated_airspeed_m_s, 0.9f, 2.0f);
 		}
 
 	} else {
-		/* no airspeed updates for one second */
-		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) {
-			_airspeed_valid = false;
+		// no airspeed updates for one second
+		if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
+			airspeed_valid = false;
 		}
 	}
 
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
+	// update TECS if validity changed
+	if (airspeed_valid != _airspeed_valid) {
+		_tecs.enable_airspeed(airspeed_valid);
+		_airspeed_valid = airspeed_valid;
+	}
 }
 
 void
@@ -820,20 +821,20 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 
 	calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 
-	// l1 navigation logic breaks down when wind speed exceeds max airspeed
-	// compute 2D groundspeed from airspeed-heading projection
-	Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
-	Vector2f nav_speed_2d{0.0f, 0.0f};
+	Vector2f nav_speed_2d{ground_speed};
 
-	// angle between air_speed_2d and ground_speed
-	float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
+	if (_airspeed_valid) {
+		// l1 navigation logic breaks down when wind speed exceeds max airspeed
+		// compute 2D groundspeed from airspeed-heading projection
+		const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
 
-	// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
-	if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
-		nav_speed_2d = air_speed_2d;
+		// angle between air_speed_2d and ground_speed
+		const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
 
-	} else {
-		nav_speed_2d = ground_speed;
+		// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
+		if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
+			nav_speed_2d = air_speed_2d;
+		}
 	}
 
 	/* no throttle limit as default */

diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -233,7 +233,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	/* throttle and airspeed states */
 	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
-	hrt_abstime _airspeed_last_received{0};			///< last time airspeed was received. Used to detect timeouts.
+	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};