control state: indicate if airspeed is not valid

msg/control_state.msg

@@ -10,6 +10,7 @@ float32 x_pos			# X position in local frame
 float32 y_pos			# Y position in local frame
 float32 z_pos			# z position in local frame
 float32 airspeed		# Airspeed, estimated
+bool airspeed_valid		# False: Non-finite values or non-updating sensor
 float32[3] vel_variance	# Variance in body velocity estimate
 float32[3] pos_variance	# Variance in local position estimate
 float32[4] q 			# Attitude Quaternion

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -623,7 +623,14 @@ void AttitudeEstimatorQ::task_main()
 		ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
 
 		/* Airspeed - take airspeed measurement directly here as no wind is estimated */
-		ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
+		if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
+		    && _airspeed.timestamp > 0) {
+			ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
+			ctrl_state.airspeed_valid = true;
+
+		} else {
+			ctrl_state.airspeed_valid = false;
+		}
 
 		/* Publish to control state topic */
 		if (_ctrl_state_pub == nullptr) {

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -896,7 +896,14 @@ void AttitudePositionEstimatorEKF::publishControlState()
 	//_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] -  _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
 	// the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL
 	// and in outdoor tests
-	_ctrl_state.airspeed = _airspeed.true_airspeed_m_s;
+	if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
+	    && _airspeed.timestamp > 0) {
+		_ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
+		_ctrl_state.airspeed_valid = true;
+
+	} else {
+		_ctrl_state.airspeed_valid = false;
+	}
 	/* Attitude Rates */
 	_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
 	_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -889,11 +889,13 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */
-				if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
+				if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) {
 					airspeed = _parameters.airspeed_trim;
+
 					if (nonfinite) {
 						perf_count(_nonfinite_input_perf);
 					}
+
 				} else {
 					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
 					airspeed = math::max(0.5f, _ctrl_state.airspeed);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -229,7 +229,7 @@ private:
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
 	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
-	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
+	uint64_t _airspeed_last_received;			///< last time airspeed was received. Used to detect timeouts.
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
 	math::Matrix<3, 3> _R_nb;			///< current attitude
@@ -558,7 +558,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	launchDetector(),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
-	_airspeed_last_valid(0),
+	_airspeed_last_received(0),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
 	_l1_control(),
@@ -807,13 +807,13 @@ FixedwingPositionControl::control_state_poll()
 
 	if (ctrl_state_updated) {
 		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
-		_airspeed_valid = true;
+		_airspeed_valid = _ctrl_state.airspeed_valid;
-		_airspeed_last_valid = hrt_absolute_time();
+		_airspeed_last_received = hrt_absolute_time();
 
 	} else {
 
 		/* no airspeed updates for one second */
-		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
+		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) {
 			_airspeed_valid = false;
 		}
 	}