From 948d24c1e3cdc3b5467fa4c82961a9e039758b06 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Thu, 10 Oct 2019 10:59:35 +0300 Subject: [PATCH] Apply suggestions from code review Co-Authored-By: Julien Lecoeur --- src/modules/mavlink/mavlink_receiver.cpp | 25 +++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 21843d1cab..e462e4c86f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1387,6 +1387,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (_vehicle_status_sub.updated()) { + _vehicle_status_sub.copy(&_vehicle_status); + } // Fill correct field by checking frametype // TODO: add as needed @@ -1397,7 +1400,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_FIXED_WING: att_sp.thrust_body[0] = set_attitude_target.thrust; - _att_sp_pub.publish(att_sp); break; case MAV_TYPE_QUADROTOR: @@ -1407,13 +1409,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_HELICOPTER: att_sp.thrust_body[2] = -set_attitude_target.thrust; - _att_sp_pub.publish(att_sp); break; case MAV_TYPE_GROUND_ROVER: att_sp.thrust_body[0] = set_attitude_target.thrust; - _att_sp_pub.publish(att_sp); break; case MAV_TYPE_VTOL_DUOROTOR: @@ -1423,23 +1423,15 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_VTOL_RESERVED3: case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED5: - if (_vehicle_status_sub.updated()) { - _vehicle_status_sub.copy(&_vehicle_status); - } - switch (_vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: att_sp.thrust_body[0] = set_attitude_target.thrust; - _fw_virtual_att_sp_pub.publish(att_sp); - break; case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: att_sp.thrust_body[2] = -set_attitude_target.thrust; - _mc_virtual_att_sp_pub.publish(att_sp); - break; default: @@ -1451,6 +1443,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } + + // Publish attitude setpoint + if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { + _mc_virtual_att_sp_pub.publish(att_sp); + + } else if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + _fw_virtual_att_sp_pub.publish(att_sp); + + } else { + _att_sp_pub.publish(att_sp); + } } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */