From fc59414a2547963224e8dd5393e843dd58929c0d Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Mon, 14 Oct 2019 16:22:59 +0300 Subject: [PATCH] set_attitude_target: set rate thrust according to VTOL mode --- src/modules/mavlink/mavlink_receiver.cpp | 32 +++++++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e462e4c86f..7e9ea4f583 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1368,6 +1368,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) _control_mode_sub.copy(&control_mode); if (control_mode.flag_control_offboard_enabled) { + if (_vehicle_status_sub.updated()) { + _vehicle_status_sub.copy(&_vehicle_status); + } /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(offboard_control_mode.ignore_attitude)) { @@ -1387,10 +1390,6 @@ 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 switch (_mavlink->get_system_type()) { @@ -1498,6 +1497,31 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_GROUND_ROVER: rates_sp.thrust_body[0] = set_attitude_target.thrust; break; + + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + case MAV_TYPE_VTOL_RESERVED2: + case MAV_TYPE_VTOL_RESERVED3: + case MAV_TYPE_VTOL_RESERVED4: + case MAV_TYPE_VTOL_RESERVED5: + switch (_vehicle_status.vehicle_type) { + case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: + rates_sp.thrust_body[0] = set_attitude_target.thrust; + + break; + + case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: + rates_sp.thrust_body[2] = -set_attitude_target.thrust; + + break; + + default: + // This should never happen + break; + } + + break; } }