From b29a6b87617b14bd5e5e875df958ee0b95631bbf Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Mon, 14 Oct 2019 18:00:03 +0300 Subject: [PATCH] set_attitude_target: get vehicle_status every time. --- src/modules/mavlink/mavlink_receiver.cpp | 13 ++++++------- src/modules/mavlink/mavlink_receiver.h | 2 -- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7e9ea4f583..7c3640740f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1368,9 +1368,8 @@ 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); - } + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(offboard_control_mode.ignore_attitude)) { @@ -1422,7 +1421,7 @@ 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: - switch (_vehicle_status.vehicle_type) { + switch (vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: att_sp.thrust_body[0] = set_attitude_target.thrust; @@ -1444,10 +1443,10 @@ 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)) { + 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)) { + } 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 { @@ -1505,7 +1504,7 @@ 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: - switch (_vehicle_status.vehicle_type) { + switch (vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: rates_sp.thrust_body[0] = set_attitude_target.thrust; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4254a02201..20b0f3fc7c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -278,8 +278,6 @@ private: hrt_abstime _last_utm_global_pos_com{0}; - vehicle_status_s _vehicle_status{}; - DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr,