delete control_state and cleanup vehicle_attitude (#7882)

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -39,6 +39,8 @@ FixedwingPositionControl *l1_control::g_control;
 static int _control_task = -1;			///< task handle for sensor task */
 
 FixedwingPositionControl::FixedwingPositionControl() :
+	_sub_airspeed(ORB_ID(airspeed), 0, 0, nullptr),
+	_sub_sensors(ORB_ID(sensor_bias), 0, 0, nullptr),
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control"))
 {
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -48,7 +50,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 	_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
-	_parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE");
+	_parameter_handles.airspeed_disabled = param_find("FW_ARSP_MODE");
 
 	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
 	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
@@ -134,7 +136,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
 	param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans));
-	param_get(_parameter_handles.airspeed_mode, &(_parameters.airspeed_mode));
+	param_get(_parameter_handles.airspeed_disabled, &(_parameters.airspeed_disabled));
 
 	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
 	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
@@ -319,27 +321,47 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 }
 
 void
-FixedwingPositionControl::control_state_poll()
+FixedwingPositionControl::airspeed_poll()
 {
-	/* check if there is a new position */
-	bool ctrl_state_updated;
-	orb_check(_ctrl_state_sub, &ctrl_state_updated);
-
-	if (ctrl_state_updated) {
-		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
-		_airspeed_valid = _ctrl_state.airspeed_valid;
+	if (!_parameters.airspeed_disabled && _sub_airspeed.updated()) {
+		_sub_airspeed.update();
+		_airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s)
+				  && PX4_ISFINITE(_sub_airspeed.get().true_airspeed_m_s);
 		_airspeed_last_received = hrt_absolute_time();
+		_airspeed = _sub_airspeed.get().indicated_airspeed_m_s;
+
+		if (_sub_airspeed.get().indicated_airspeed_m_s > 0.0f
+		    && _sub_airspeed.get().true_airspeed_m_s > _sub_airspeed.get().indicated_airspeed_m_s) {
+			_eas2tas = max(_sub_airspeed.get().true_airspeed_m_s / _sub_airspeed.get().indicated_airspeed_m_s, 1.0f);
+
+		} else {
+			_eas2tas = 1.0f;
+		}
 
 	} else {
-
 		/* no airspeed updates for one second */
 		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) {
 			_airspeed_valid = false;
 		}
 	}
 
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
+}
+
+void
+FixedwingPositionControl::vehicle_attitude_poll()
+{
+	/* check if there is a new position */
+	bool updated;
+	orb_check(_vehicle_attitude_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
+	}
+
 	/* set rotation matrix and euler angles */
-	math::Quaternion q_att(_ctrl_state.q);
+	math::Quaternion q_att(_att.q);
 	_R_nb = q_att.to_dcm();
 
 	math::Vector<3> euler_angles;
@@ -347,9 +369,6 @@ FixedwingPositionControl::control_state_poll()
 	_roll    = euler_angles(0);
 	_pitch   = euler_angles(1);
 	_yaw     = euler_angles(2);
-
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
 }
 
 void
@@ -650,7 +669,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 	_att_sp.apply_flaps = false;		// by default we don't use flaps
 
 	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body(_ctrl_state.x_acc, _ctrl_state.y_acc, _ctrl_state.z_acc);
+	math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
 
 	// tailsitters use the multicopter frame as reference, in fixed wing
 	// we need to use the fixed wing frame
@@ -669,14 +688,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 				    _control_mode.flag_control_altitude_enabled));
 
 	/* update TECS filters */
-	_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
+	_tecs.update_state(_global_pos.alt, _airspeed, _R_nb,
 			   accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
 
 	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
-	math::Vector<2> air_speed_2d{_ctrl_state.airspeed * cosf(_yaw), _ctrl_state.airspeed * sinf(_yaw)};
+	math::Vector<2> air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
 	math::Vector<2> nav_speed_2d{0.0f, 0.0f};
 
 	// angle between air_speed_2d and ground_speed
@@ -1063,8 +1082,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 			if (_runway_takeoff.runwayTakeoffEnabled()) {
 				if (!_runway_takeoff.isInitialized()) {
-					Eulerf euler(Quatf(_ctrl_state.q));
-					_runway_takeoff.init(euler.psi(), _global_pos.lat, _global_pos.lon);
+					_runway_takeoff.init(_yaw, _global_pos.lat, _global_pos.lon);
 
 					/* need this already before takeoff is detected
 					 * doesn't matter if it gets reset when takeoff is detected eventually */
@@ -1076,7 +1094,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 				float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
 
 				// update runway takeoff helper
-				_runway_takeoff.update(_ctrl_state.airspeed, _global_pos.alt - terrain_alt,
+				_runway_takeoff.update(_airspeed, _global_pos.alt - terrain_alt,
 						       _global_pos.lat, _global_pos.lon, &_mavlink_log_pub);
 
 				/*
@@ -1124,8 +1142,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 							_launch_detection_notify = hrt_absolute_time();
 						}
 
-						/* Detect launch */
-						_launchDetector.update(_ctrl_state.x_acc);
+						/* Detect launch using body X (forward) acceleration */
+						_launchDetector.update(_sub_sensors.get().accel_x);
 
 						/* update our copy of the launch detection state */
 						_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1271,7 +1289,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 		    fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
 
 			/* heading / roll is zero, lock onto current heading */
-			if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
+			if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
 				// little yaw movement, lock to current heading
 				_yaw_lock_engaged = true;
 
@@ -1497,8 +1515,8 @@ FixedwingPositionControl::task_main()
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
@@ -1550,6 +1568,7 @@ FixedwingPositionControl::task_main()
 		vehicle_command_poll();
 		vehicle_land_detected_poll();
 		vehicle_status_poll();
+		_sub_sensors.update();
 
 		/* only update parameters if they changed */
 		bool params_updated = false;
@@ -1592,7 +1611,8 @@ FixedwingPositionControl::task_main()
 			_alt_reset_counter = _global_pos.alt_reset_counter;
 			_pos_reset_counter = _global_pos.lat_lon_reset_counter;
 
-			control_state_poll();
+			airspeed_poll();
+			vehicle_attitude_poll();
 			manual_control_setpoint_poll();
 			position_setpoint_triplet_poll();
 
@@ -1741,12 +1761,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 			_was_in_transition = true;
 
 			// set this to transition airspeed to init tecs correctly
-			if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
+			if (_parameters.airspeed_disabled) {
 				// some vtols fly without airspeed sensor
 				_asp_after_transition = _parameters.airspeed_trans;
 
 			} else {
-				_asp_after_transition = _ctrl_state.airspeed;
+				_asp_after_transition = _airspeed;
 			}
 
 			_asp_after_transition = constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
@@ -1755,8 +1775,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
 			_asp_after_transition += dt * 2; // increase 2m/s
 
-			if (_asp_after_transition < airspeed_sp && _ctrl_state.airspeed < airspeed_sp) {
-				airspeed_sp = max(_asp_after_transition, _ctrl_state.airspeed);
+			if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
+				airspeed_sp = max(_asp_after_transition, _airspeed);
 
 			} else {
 				_was_in_transition = false;
@@ -1801,11 +1821,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 		pitch_for_tecs = euler(1);
 	}
 
-	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
-
 	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
 				    _global_pos.alt, alt_sp,
-				    airspeed_sp, _ctrl_state.airspeed, eas2tas,
+				    airspeed_sp, _airspeed, _eas2tas,
 				    climbout_mode, climbout_pitch_min_rad,
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad, pitch_max_rad);

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -72,12 +72,15 @@
 #include <mathlib/mathlib.h>
 #include <runway_takeoff/RunwayTakeoff.h>
 #include <systemlib/perf_counter.h>
-#include <uORB/topics/control_state.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -87,15 +90,22 @@
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
-#define HDG_HOLD_DIST_NEXT 		3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
-#define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
-#define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
-#define HDG_HOLD_YAWRATE_THRESH 	0.15f 		// max yawrate at which plane locks yaw for heading hold mode
-#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll/yaw input from user which does not change the locked heading
-#define T_ALT_TIMEOUT 			1		// time after which we abort landing if terrain estimate is not valid
-#define THROTTLE_THRESH 0.05f				///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
-#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f		///< a throttle / pitch input above this value leads to the system switching to climbout mode
-#define ALTHOLD_EPV_RESET_THRESH 5.0f
+static constexpr float HDG_HOLD_DIST_NEXT =
+	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
+static constexpr float HDG_HOLD_REACHED_DIST =
+	1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
+static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;	// max yawrate at which plane locks yaw for heading hold mode
+static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
+	0.01f; // max manual roll/yaw input from user which does not change the locked heading
+
+static constexpr hrt_abstime T_ALT_TIMEOUT = 1; // time after which we abort landing if terrain estimate is not valid
+
+static constexpr float THROTTLE_THRESH =
+	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
+	0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
+static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
 
 using math::constrain;
 using math::max;
@@ -108,6 +118,8 @@ using matrix::Quatf;
 using matrix::Vector2f;
 using matrix::Vector3f;
 
+using uORB::Subscription;
+
 using namespace launchdetection;
 using namespace runwaytakeoff;
 
@@ -141,8 +153,8 @@ private:
 
 	int		_global_pos_sub{-1};
 	int		_pos_sp_triplet_sub{-1};
-	int		_ctrl_state_sub{-1};			///< control state subscription */
 	int		_control_mode_sub{-1};			///< control mode subscription */
+	int		_vehicle_attitude_sub{-1};		///< vehicle attitude subscription */
 	int		_vehicle_command_sub{-1};		///< vehicle command subscription */
 	int		_vehicle_status_sub{-1};		///< vehicle status subscription */
 	int		_vehicle_land_detected_sub{-1};		///< vehicle land detected subscription */
@@ -155,10 +167,10 @@ private:
 
 	orb_id_t _attitude_setpoint_id{nullptr};
 
-	control_state_s			_ctrl_state {};			///< control state */
 	fw_pos_ctrl_status_s		_fw_pos_ctrl_status {};		///< navigation capabilities */
 	manual_control_setpoint_s	_manual {};			///< r/c channel data */
 	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items */
+	vehicle_attitude_s	_att {};			///< vehicle attitude setpoint */
 	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint */
 	vehicle_command_s		_vehicle_command {};		///< vehicle commands */
 	vehicle_control_mode_s		_control_mode {};		///< control mode */
@@ -166,6 +178,9 @@ private:
 	vehicle_land_detected_s		_vehicle_land_detected {};	///< vehicle land detected */
 	vehicle_status_s		_vehicle_status {};		///< vehicle status */
 
+	Subscription<airspeed_s> _sub_airspeed;
+	Subscription<sensor_bias_s> _sub_sensors;
+
 	perf_counter_t	_loop_perf;				///< loop performance counter */
 
 	float	_hold_alt{0.0f};				///< hold altitude for altitude mode */
@@ -214,6 +229,8 @@ private:
 	/* 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.
+	float _airspeed{0.0f};
+	float _eas2tas{1.0f};
 
 	float _groundspeed_undershoot{0.0f};			///< ground speed error to min. speed in m/s
 
@@ -269,7 +286,7 @@ private:
 		float airspeed_trim;
 		float airspeed_max;
 		float airspeed_trans;
-		int32_t airspeed_mode;
+		int32_t airspeed_disabled;
 
 		float pitch_limit_min;
 		float pitch_limit_max;
@@ -327,7 +344,7 @@ private:
 		param_t airspeed_trim;
 		param_t airspeed_max;
 		param_t airspeed_trans;
-		param_t airspeed_mode;
+		param_t airspeed_disabled;
 
 		param_t pitch_limit_min;
 		param_t pitch_limit_max;
@@ -366,10 +383,11 @@ private:
 	int		parameters_update();
 
 	// Update subscriptions
-	void		control_state_poll();
+	void		airspeed_poll();
 	void		control_update();
 	void		manual_control_setpoint_poll();
 	void		position_setpoint_triplet_poll();
+	void		vehicle_attitude_poll();
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
 	void		vehicle_land_detected_poll();