Browse Source

Mavlink receiver SET_ATTITUDE_TARGET fill the correct throttle field by checking its frametype in offboard mode (#12149)

sbg
SalimTerryLi 6 years ago committed by Daniel Agar
parent
commit
e0f3fc8d00
  1. 26
      src/modules/mavlink/mavlink_receiver.cpp

26
src/modules/mavlink/mavlink_receiver.cpp

@ -1365,11 +1365,29 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.yaw_sp_move_rate = 0.0f; att_sp.yaw_sp_move_rate = 0.0f;
} }
// TODO: We assume offboard is only used for multicopters which produce thrust along the
// body z axis. If we want to support fixed wing as well we need to handle it differently here, e.g.
// in that case we should assign att_sp.thrust_body[0]
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
att_sp.thrust_body[2] = -set_attitude_target.thrust; // Fill correct field by checking frametype
// TODO: add as needed
switch (_mavlink->get_system_type()) {
case MAV_TYPE_GENERIC:
break;
case MAV_TYPE_FIXED_WING:
att_sp.thrust_body[0] = set_attitude_target.thrust;
break;
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR:
case MAV_TYPE_TRICOPTER:
case MAV_TYPE_HELICOPTER:
att_sp.thrust_body[2] = -set_attitude_target.thrust;
break;
case MAV_TYPE_GROUND_ROVER:
att_sp.thrust_body[0] = set_attitude_target.thrust;
break;
}
} }
if (_att_sp_pub == nullptr) { if (_att_sp_pub == nullptr) {

Loading…
Cancel
Save