added airspeed mode enum to control state topic

Signed-off-by: Roman <bapstr@ethz.ch>

msg/control_state.msg

@@ -1,4 +1,8 @@
 # This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
+uint8 AIRSPD_MODE_MEAS = 0	# airspeed is measured airspeed from sensor
+uint8 AIRSPD_MODE_EST = 1	# airspeed is estimated by body velocity
+uint8 AIRSPD_MODE_DISABLED = 2	# airspeed is disabled
+
 float32 x_acc			# X acceleration in body frame
 float32 y_acc			# Y acceleration in body frame
 float32 z_acc			# Z acceleration in body frame

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -631,25 +631,25 @@ void AttitudeEstimatorQ::task_main()
 
 			ctrl_state.airspeed_valid = false;
 
-			// use estimated velocity for airspeed estimate
-			if (_airspeed_mode == 1) {
-				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
-					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
-				}
-
-				// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed
-
-			} else if (_airspeed_mode == 2) {
-
-				// use the measured airspeed
-			} else {
-				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+			if (_airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
+				// use measured airspeed
 				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 if (_airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
+				// use estimated body velocity as airspeed estimate
+				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
+					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
+					ctrl_state.airspeed_valid = true;
 				}
+
+			} else if (_airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
+				// do nothing, airspeed has been declared as non-valid above, controllers
+				// will handle this assuming always trim airspeed
 			}
 
 			/* the instance count is not used here */

src/modules/ekf2/ekf2_main.cpp

@@ -665,24 +665,24 @@ void Ekf2::task_main()
 			ctrl_state.airspeed_valid = false;
 
 			// use estimated velocity for airspeed estimate
-			if (_airspeed_mode.get() == 1) {
-				if (_ekf.local_position_is_valid()) {
-					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
-				}
-
-				// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed
-
-			} else if (_airspeed_mode.get() == 2) {
-
-				// use the measured airspeed
-			} else {
-				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+			if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
+				// use measured airspeed
 				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;
 				}
 			}
+			if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
+				if (_ekf.local_position_is_valid()) {
+					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
+					ctrl_state.airspeed_valid = true;
+				}
+
+			} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
+				// do nothing, airspeed has been declared as non-valid above, controllers
+				// will handle this assuming always trim airspeed
+			}
 
 			// publish control state data
 			if (_control_state_pub == nullptr) {

src/modules/vtol_att_control/standard.cpp

@@ -187,7 +187,8 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if (((_params_standard.airspeed_mode == 2 || _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
+			if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED ||
+				_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
 			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
 			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
 			    can_transition_on_ground()) {
@@ -250,7 +251,7 @@ void Standard::update_transition_state()
 			_mc_throttle_weight = weight;
 
 		// time based blending when no airspeed sensor is set
-		} else if (_params_standard.airspeed_mode == 2 &&
+		} else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED &&
 				  (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
 				  ) {
 			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_time_min * 1000000.0f));

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -84,6 +84,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/control_state.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>