VTOL coordinate FW and MC rate controllers only

msg/vehicle_attitude_setpoint.msg

@@ -11,7 +11,9 @@ float32 yaw_sp_move_rate		# rad/s (commanded by user)
 float32[4] q_d					# Desired quaternion for quaternion control
 bool q_d_valid					# Set to true if quaternion vector is valid
 
-float32 thrust					# Thrust in Newton the power system should generate
+float32 thrust_x					# Thrust in Newton the power system should generate
+float32 thrust_y					# Thrust in Newton the power system should generate
+float32 thrust_z					# Thrust in Newton the power system should generate
 
 bool roll_reset_integral			# Reset roll integral part (navigation logic change)
 bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)

msg/vehicle_rates_setpoint.msg

@@ -2,6 +2,7 @@
 float32 roll	    # body angular rates in NED frame
 float32 pitch	    # body angular rates in NED frame
 float32 yaw			# body angular rates in NED frame
-float32 thrust	    # thrust normalized to 0..1
 
-# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint
+float32 thrust_x	    # thrust normalized to 0..1
+float32 thrust_y	    # thrust normalized to 0..1
+float32 thrust_z	    # thrust normalized to 0..1

src/examples/rover_steering_control/main.cpp

@@ -183,7 +183,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	actuators->control[2] = yaw_err * pp.yaw_p;
 
 	/* copy throttle */
-	actuators->control[3] = att_sp->thrust;
+	actuators->control[3] = att_sp->thrust_x;
 
 	actuators->timestamp = hrt_absolute_time();
 }

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -249,7 +249,6 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
-
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
 	}
 }
@@ -263,7 +262,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	orb_check(_manual_sub, &manual_updated);
 
 	// only update manual if in a manual mode
-	if (_vcontrol_mode.flag_control_manual_enabled && manual_updated) {
+	if (_vcontrol_mode.flag_control_manual_enabled && manual_updated && !_vehicle_status.is_rotary_wing) {
 
 		if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {
 
@@ -285,7 +284,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 					_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
 					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
 					_att_sp.yaw_body = 0.0f;
-					_att_sp.thrust = _manual.z;
+					_att_sp.thrust_x = _manual.z;
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
@@ -308,15 +307,15 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 					_rates_sp.roll = _manual.y * _parameters.acro_max_x_rate_rad;
 					_rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad;
 					_rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad;
-					_rates_sp.thrust = _manual.z;
+					_rates_sp.thrust_x = _manual.z;
 
 					if (_rate_sp_pub != nullptr) {
 						/* publish the attitude rates setpoint */
-						orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
 
-					} else if (_rates_sp_id) {
+					} else {
 						/* advertise the attitude rates setpoint */
-						_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
+						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
 					}
 
 				} else {
@@ -333,14 +332,30 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 }
 
 void
-FixedwingAttitudeControl::vehicle_setpoint_poll()
+FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
 {
 	/* check if there is a new setpoint */
 	bool att_sp_updated;
 	orb_check(_att_sp_sub, &att_sp_updated);
 
 	if (att_sp_updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+		if (orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp)) {
+			_rates_sp.thrust_x = _att_sp.thrust_x;
+			_rates_sp.thrust_y = _att_sp.thrust_y;
+			_rates_sp.thrust_z = _att_sp.thrust_z;
+		}
+	}
+}
+
+void
+FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool rates_sp_updated = false;
+	orb_check(_rates_sp_sub, &rates_sp_updated);
+
+	if (rates_sp_updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
 	}
 }
 
@@ -366,10 +381,17 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 
+		// if VTOL listen to rate setpoints only
+		if (_vehicle_status.is_vtol) {
+			if (_vehicle_status.is_rotary_wing) {
+				_vcontrol_mode.flag_control_attitude_enabled = false;
+				_vcontrol_mode.flag_control_manual_enabled = false;
+			}
+		}
+
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (!_rates_sp_id) {
+		if (!_actuators_id) {
 			if (_vehicle_status.is_vtol) {
-				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
 				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 
@@ -378,7 +400,6 @@ FixedwingAttitudeControl::vehicle_status_poll()
 				parameters_update();
 
 			} else {
-				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_0);
 				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
 			}
@@ -420,7 +441,7 @@ void FixedwingAttitudeControl::run()
 	parameters_update();
 
 	/* get an initial update for all sensor and status data */
-	vehicle_setpoint_poll();
+	vehicle_attitude_setpoint_poll();
 	vehicle_control_mode_poll();
 	vehicle_manual_poll();
 	vehicle_status_poll();
@@ -527,7 +548,7 @@ void FixedwingAttitudeControl::run()
 			matrix::Eulerf euler_angles(R);
 
 			_airspeed_sub.update();
-			vehicle_setpoint_poll();
+			vehicle_attitude_setpoint_poll();
 			vehicle_control_mode_poll();
 			vehicle_manual_poll();
 			global_pos_poll();
@@ -618,10 +639,6 @@ void FixedwingAttitudeControl::run()
 					_wheel_ctrl.reset_integrator();
 				}
 
-				float roll_sp = _att_sp.roll_body;
-				float pitch_sp = _att_sp.pitch_body;
-				float yaw_sp = _att_sp.yaw_body;
-
 				/* Prepare data for attitude controllers */
 				struct ECL_ControlData control_input = {};
 				control_input.roll = euler_angles.phi();
@@ -630,9 +647,9 @@ void FixedwingAttitudeControl::run()
 				control_input.body_x_rate = _att.rollspeed;
 				control_input.body_y_rate = _att.pitchspeed;
 				control_input.body_z_rate = _att.yawspeed;
-				control_input.roll_setpoint = roll_sp;
-				control_input.pitch_setpoint = pitch_sp;
-				control_input.yaw_setpoint = yaw_sp;
+				control_input.roll_setpoint = _att_sp.roll_body;
+				control_input.pitch_setpoint = _att_sp.pitch_body;
+				control_input.yaw_setpoint = _att_sp.yaw_body;
 				control_input.airspeed_min = _parameters.airspeed_min;
 				control_input.airspeed_max = _parameters.airspeed_max;
 				control_input.airspeed = airspeed;
@@ -687,7 +704,7 @@ void FixedwingAttitudeControl::run()
 
 				/* Run attitude controllers */
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
-					if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
+					if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
 						_roll_ctrl.control_attitude(control_input);
 						_pitch_ctrl.control_attitude(control_input);
 						_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
@@ -738,8 +755,8 @@ void FixedwingAttitudeControl::run()
 						}
 
 						/* throttle passed through if it is finite and if no engine failure was detected */
-						_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust)
-								&& !_vehicle_status.engine_failure) ? _att_sp.thrust : 0.0f;
+						_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_x)
+								&& !_vehicle_status.engine_failure) ? _att_sp.thrust_x : 0.0f;
 
 						/* scale effort by battery status */
 						if (_parameters.bat_scale_en &&
@@ -777,11 +794,11 @@ void FixedwingAttitudeControl::run()
 
 					if (_rate_sp_pub != nullptr) {
 						/* publish the attitude rates setpoint */
-						orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
 
-					} else if (_rates_sp_id) {
+					} else {
 						/* advertise the attitude rates setpoint */
-						_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
+						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
 					}
 
 				} else {
@@ -799,7 +816,7 @@ void FixedwingAttitudeControl::run()
 					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
 					_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
 
-					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust) ? _rates_sp.thrust : 0.0f;
+					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_x) ? _rates_sp.thrust_x : 0.0f;
 				}
 
 				rate_ctrl_status_s rate_ctrl_status;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -94,6 +94,7 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 
 	int		_att_sub{-1};				/**< vehicle attitude */
 	int		_att_sp_sub{-1};			/**< vehicle attitude setpoint */
+	int		_rates_sp_sub{-1};			/**< vehicle attitude setpoint */
 	int		_battery_status_sub{-1};		/**< battery status subscription */
 	int		_global_pos_sub{-1};			/**< global position subscription */
 	int		_manual_sub{-1};			/**< notification of manual control updates */
@@ -108,7 +109,6 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	orb_advert_t	_actuators_2_pub{nullptr};		/**< actuator control group 1 setpoint (Airframe) */
 	orb_advert_t	_rate_ctrl_status_pub{nullptr};		/**< rate controller status publication */
 
-	orb_id_t _rates_sp_id{nullptr};	// pointer to correct rates setpoint uORB metadata structure
 	orb_id_t _actuators_id{nullptr};	// pointer to correct actuator controls0 uORB metadata structure
 	orb_id_t _attitude_setpoint_id{nullptr};
 
@@ -286,7 +286,8 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 
 	void		vehicle_control_mode_poll();
 	void		vehicle_manual_poll();
-	void		vehicle_setpoint_poll();
+	void		vehicle_attitude_setpoint_poll();
+	void		vehicle_rates_setpoint_poll();
 	void		global_pos_poll();
 	void		vehicle_status_poll();
 	void		vehicle_land_detected_poll();

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -742,7 +742,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 		}
 
 		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-			_att_sp.thrust = 0.0f;
+			_att_sp.thrust_x = 0.0f;
 			_att_sp.roll_body = 0.0f;
 			_att_sp.pitch_body = 0.0f;
 
@@ -999,30 +999,30 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 		/* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons.
 		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust = _parameters.throttle_idle;
+		_att_sp.thrust_x = _parameters.throttle_idle;
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _runway_takeoff.runwayTakeoffEnabled()) {
 
-		_att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
+		_att_sp.thrust_x = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 
-		_att_sp.thrust = 0.0f;
+		_att_sp.thrust_x = 0.0f;
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
-		_att_sp.thrust = min(_att_sp.thrust, _parameters.throttle_max);
+		_att_sp.thrust_x = min(_att_sp.thrust_x, _parameters.throttle_max);
 
 	} else {
 		/* Copy thrust and pitch values from tecs */
 		if (_vehicle_land_detected.landed) {
 			// when we are landed state we want the motor to spin at idle speed
-			_att_sp.thrust = min(_parameters.throttle_idle, throttle_max);
+			_att_sp.thrust_x = min(_parameters.throttle_idle, throttle_max);
 
 		} else {
-			_att_sp.thrust = min(get_tecs_thrust(), throttle_max);
+			_att_sp.thrust_x = min(get_tecs_thrust(), throttle_max);
 		}
 	}
 

src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp

@@ -302,7 +302,7 @@ GroundRoverAttitudeControl::task_main()
 					}
 
 					/* throttle passed through if it is finite and if no engine failure was detected */
-					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust;
+					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust_x;
 
 					/* scale effort by battery status */
 					if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&

src/modules/gnd_pos_control/GroundRoverPositionControl.cpp

@@ -279,7 +279,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
 			_att_sp.roll_body = 0.0f;
 			_att_sp.pitch_body = 0.0f;
 			_att_sp.yaw_body = 0.0f;
-			_att_sp.thrust = 0.0f;
+			_att_sp.thrust_x = 0.0f;
 
 		} else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)
 			   || (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
@@ -290,7 +290,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
 			_att_sp.pitch_body = 0.0f;
 			_att_sp.yaw_body = _gnd_control.nav_bearing();
 			_att_sp.fw_control_yaw = true;
-			_att_sp.thrust = mission_throttle;
+			_att_sp.thrust_x = mission_throttle;
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
@@ -302,7 +302,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
 			_att_sp.pitch_body = 0.0f;
 			_att_sp.yaw_body = _gnd_control.nav_bearing();
 			_att_sp.fw_control_yaw = true;
-			_att_sp.thrust = 0.0f;
+			_att_sp.thrust_x = 0.0f;
 		}
 
 		if (was_circle_mode && !_gnd_control.circle_mode()) {
@@ -317,7 +317,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
 		_att_sp.pitch_body = 0.0f;
 		_att_sp.yaw_body = 0.0f;
 		_att_sp.fw_control_yaw = true;
-		_att_sp.thrust = 0.0f;
+		_att_sp.thrust_x = 0.0f;
 
 		/* do not publish the setpoint */
 		setpoint = false;

src/modules/mavlink/mavlink_messages.cpp

@@ -2815,7 +2815,7 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream
 			msg.body_pitch_rate = att_rates_sp.pitch;
 			msg.body_yaw_rate = att_rates_sp.yaw;
 
-			msg.thrust = att_sp.thrust;
+			msg.thrust = att_sp.thrust_x;
 
 			mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -1297,7 +1297,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 
 					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						att_sp.thrust = set_attitude_target.thrust;
+						att_sp.thrust_x = set_attitude_target.thrust;
 					}
 
 					if (_att_sp_pub == nullptr) {
@@ -1321,7 +1321,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 
 					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						rates_sp.thrust = set_attitude_target.thrust;
+						rates_sp.thrust_z = set_attitude_target.thrust;
 					}
 
 					if (_rates_sp_pub == nullptr) {

src/modules/mc_att_control/mc_att_control.hpp

@@ -144,7 +144,6 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
 	orb_advert_t	_controller_status_pub{nullptr};	/**< controller status publication */
 
-	orb_id_t _rates_sp_id{nullptr};		/**< pointer to correct rates setpoint uORB metadata structure */
 	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
 
 	bool		_actuators_0_circuit_breaker_enabled{false};	/**< circuit breaker to suppress output */
@@ -174,7 +173,7 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	matrix::Vector3f _rates_prev_filtered;		/**< angular rates on previous step (low-pass filtered) */
 	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */
 	matrix::Vector3f _rates_int;			/**< angular rates integral error */
-	float _thrust_sp;				/**< thrust setpoint */
+
 	matrix::Vector3f _att_control;			/**< attitude control vector */
 
 	matrix::Dcmf _board_rotation;			/**< rotation matrix for the orientation that the board is mounted */