fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

set_attitude_target: get vehicle_status every time.

Browse Source
This commit is contained in:
Ildar Sadykov
2019-10-26 18:29:16 +01:00
committed by Nuno Marques
parent fc59414a25
commit b29a6b8761
2 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+6 -7
View File
@@ -1368,9 +1368,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
_control_mode_sub.copy(&control_mode); _control_mode_sub.copy(&control_mode);
if (control_mode.flag_control_offboard_enabled) { if (control_mode.flag_control_offboard_enabled) {
if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&_vehicle_status); _vehicle_status_sub.copy(&vehicle_status);
}
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */ /* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_mode.ignore_attitude)) { 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_RESERVED3:
case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED4:
case MAV_TYPE_VTOL_RESERVED5: case MAV_TYPE_VTOL_RESERVED5:
switch (_vehicle_status.vehicle_type) { switch (vehicle_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
att_sp.thrust_body[0] = set_attitude_target.thrust; 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 // 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); _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); _fw_virtual_att_sp_pub.publish(att_sp);
} else { } else {
@@ -1505,7 +1504,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
case MAV_TYPE_VTOL_RESERVED3: case MAV_TYPE_VTOL_RESERVED3:
case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED4:
case MAV_TYPE_VTOL_RESERVED5: case MAV_TYPE_VTOL_RESERVED5:
switch (_vehicle_status.vehicle_type) { switch (vehicle_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
rates_sp.thrust_body[0] = set_attitude_target.thrust; rates_sp.thrust_body[0] = set_attitude_target.thrust;
src/modules/mavlink/mavlink_receiver.h
-2
View File
@@ -278,8 +278,6 @@ private:
hrt_abstime _last_utm_global_pos_com{0}; hrt_abstime _last_utm_global_pos_com{0};
vehicle_status_s _vehicle_status{};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr, (ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr, (ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,