mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: break out a method for set-attitude-target
This commit is contained in:
parent
f96add13e9
commit
127961e692
@ -1235,6 +1235,24 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
|
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
|
||||||
|
handle_set_attitude_target(msg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
|
||||||
|
handle_set_position_target_local_ned(msg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
|
||||||
|
handle_set_position_target_global_int(msg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
GCS_MAVLINK::handle_message(msg);
|
||||||
|
break;
|
||||||
|
} // end switch
|
||||||
|
} // end handle mavlink
|
||||||
|
|
||||||
|
void GCS_MAVLINK_Plane::handle_set_attitude_target(const mavlink_message_t &msg)
|
||||||
{
|
{
|
||||||
// Only allow companion computer (or other external controller) to
|
// Only allow companion computer (or other external controller) to
|
||||||
// control attitude in GUIDED mode. We DON'T want external control
|
// control attitude in GUIDED mode. We DON'T want external control
|
||||||
@ -1242,7 +1260,7 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
|
|||||||
// computer control is more safe (even more so when using
|
// computer control is more safe (even more so when using
|
||||||
// FENCE_ACTION = 4 for geofence failures).
|
// FENCE_ACTION = 4 for geofence failures).
|
||||||
if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes
|
if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes
|
||||||
break;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
mavlink_set_attitude_target_t att_target;
|
mavlink_set_attitude_target_t att_target;
|
||||||
@ -1300,24 +1318,8 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
|
|||||||
// Update timer for external throttle
|
// Update timer for external throttle
|
||||||
plane.guided_state.last_forced_throttle_ms = now;
|
plane.guided_state.last_forced_throttle_ms = now;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
|
|
||||||
handle_set_position_target_local_ned(msg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
|
|
||||||
handle_set_position_target_global_int(msg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
GCS_MAVLINK::handle_message(msg);
|
|
||||||
break;
|
|
||||||
} // end switch
|
|
||||||
} // end handle mavlink
|
|
||||||
|
|
||||||
void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg)
|
void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg)
|
||||||
{
|
{
|
||||||
// decode packet
|
// decode packet
|
||||||
|
@ -68,6 +68,7 @@ private:
|
|||||||
|
|
||||||
void handle_set_position_target_global_int(const mavlink_message_t &msg);
|
void handle_set_position_target_global_int(const mavlink_message_t &msg);
|
||||||
void handle_set_position_target_local_ned(const mavlink_message_t &msg);
|
void handle_set_position_target_local_ned(const mavlink_message_t &msg);
|
||||||
|
void handle_set_attitude_target(const mavlink_message_t &msg);
|
||||||
|
|
||||||
#if HAL_QUADPLANE_ENABLED
|
#if HAL_QUADPLANE_ENABLED
|
||||||
#if AP_MAVLINK_COMMAND_LONG_ENABLED
|
#if AP_MAVLINK_COMMAND_LONG_ENABLED
|
||||||
|
Loading…
Reference in New Issue
Block a user