dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Apply suggestions from code review 

Co-Authored-By: Julien Lecoeur <jlecoeur@users.noreply.github.com>
This commit is contained in:
Ildar Sadykov 2019-10-10 10:59:35 +03:00 committed by Nuno Marques
parent 65ed068bbd
commit 948d24c1e3
1 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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 */