diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index a42b87a820..f58e8c1eb9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1365,11 +1365,29 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 						att_sp.yaw_sp_move_rate = 0.0f;
 					}
 
-					// TODO: We assume offboard is only used for multicopters which produce thrust along the
-					// body z axis. If we want to support fixed wing as well we need to handle it differently here, e.g.
-					// in that case we should assign att_sp.thrust_body[0]
 					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						att_sp.thrust_body[2] = -set_attitude_target.thrust;
+						// 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_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;
+						}
 					}
 
 					if (_att_sp_pub == nullptr) {