From c8edac0a1d3b0f29813f6cd35dcdf338ab780863 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <sadykov@arrival.com>
Date: Wed, 9 Oct 2019 16:09:33 +0300
Subject: [PATCH] VTOL state check for attitude setpoint publish

---
 src/modules/mavlink/mavlink_receiver.cpp | 62 ++++++++++++++----------
 src/modules/mavlink/mavlink_receiver.h   |  5 ++
 2 files changed, 42 insertions(+), 25 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 511b011d2e..36ddb2ba68 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -99,6 +99,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	} else {
 		_is_vtol = false;
 	}
+
 }
 
 void
@@ -1401,36 +1402,47 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 
 					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						// Fill correct field by checking frametype
-						// TODO: add as needed
-						switch (_mavlink->get_system_type()) {
-						case MAV_TYPE_GENERIC:
-							break;
+						if (!_is_vtol) {
+							// Fill correct field by checking frametype
+							// TODO: add as needed
+							switch (_mavlink->get_system_type()) {
+							case MAV_TYPE_GENERIC:
+								break;
 
-						case MAV_TYPE_FIXED_WING:
-							att_sp.thrust_body[0] = set_attitude_target.thrust;
-							break;
+							case MAV_TYPE_FIXED_WING:
+								att_sp.thrust_body[0] = set_attitude_target.thrust;
+								break;
 
-						case MAV_TYPE_QUADROTOR:
-						case MAV_TYPE_HEXAROTOR:
-						case MAV_TYPE_OCTOROTOR:
-						case MAV_TYPE_TRICOPTER:
-						case MAV_TYPE_HELICOPTER:
-							att_sp.thrust_body[2] = -set_attitude_target.thrust;
-							break;
+							case MAV_TYPE_QUADROTOR:
+							case MAV_TYPE_HEXAROTOR:
+							case MAV_TYPE_OCTOROTOR:
+							case MAV_TYPE_TRICOPTER:
+							case MAV_TYPE_HELICOPTER:
+								att_sp.thrust_body[2] = -set_attitude_target.thrust;
+								break;
 
-						case MAV_TYPE_GROUND_ROVER:
-							att_sp.thrust_body[0] = set_attitude_target.thrust;
-							break;
+							case MAV_TYPE_GROUND_ROVER:
+								att_sp.thrust_body[0] = set_attitude_target.thrust;
+								break;
+							}
+
+							_att_sp_pub.publish(att_sp);
+
+						} else {
+							if (_vtol_vehicle_status_sub.updated()) {
+								_vtol_vehicle_status_sub.copy(&_vtol_vehicle_status);
+							}
+
+							if (_vtol_vehicle_status.vtol_in_rw_mode) {
+								att_sp.thrust_body[2] = -set_attitude_target.thrust;
+								_mc_virtual_att_sp_pub.publish(att_sp);
+
+							} else {
+								att_sp.thrust_body[0] = set_attitude_target.thrust;
+								_fw_virtual_att_sp_pub.publish(att_sp);
+							}
 						}
 					}
-
-					if (!_is_vtol) {
-						_att_sp_pub.publish(att_sp);
-
-					} else {
-						_mc_virtual_att_sp_pub.publish(att_sp);
-					}
 				}
 
 				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 14cfde2055..c0e60eaae2 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -97,6 +97,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 
 class Mavlink;
 
@@ -230,6 +231,7 @@ private:
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
+	uORB::Publication<vehicle_attitude_setpoint_s>		_fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
 	uORB::Publication<vehicle_global_position_s>		_global_pos_pub{ORB_ID(vehicle_global_position)};
 	uORB::Publication<vehicle_gps_position_s>		_gps_pub{ORB_ID(vehicle_gps_position)};
 	uORB::Publication<vehicle_land_detected_s>		_land_detector_pub{ORB_ID(vehicle_land_detected)};
@@ -263,6 +265,7 @@ private:
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription	_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription	_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
 	static constexpr unsigned int	MOM_SWITCH_COUNT{8};
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
@@ -278,6 +281,8 @@ private:
 
 	bool _is_vtol{false};
 
+	vtol_vehicle_status_s _vtol_vehicle_status{};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,