Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: break out a method for set-position-target-local-ned

This commit is contained in:
Peter Barker 2024-01-24 10:07:19 +11:00 committed by Andrew Tridgell
parent 5bc2b1a09b
commit f96add13e9
2 changed files with 24 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/GCS_Mavlink.cpp
View File

@ -1305,28 +1305,8 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
// exit if vehicle is not in Guided mode
if (plane.control_mode != &plane.mode_guided) {
handle_set_position_target_local_ned(msg);
break;
}
// only local moves for now
if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
break;
}
// just do altitude for now
plane.next_WP_loc.alt += -packet.z*100.0;
gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
(double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
handle_set_position_target_global_int(msg);
@ -1338,6 +1318,28 @@ void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
} // end switch
} // end handle mavlink
void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg)
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
// exit if vehicle is not in Guided mode
if (plane.control_mode != &plane.mode_guided) {
return;
}
// only local moves for now
if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
return;
}
// just do altitude for now
plane.next_WP_loc.alt += -packet.z*100.0;
gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
(double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
}
void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_message_t &msg)
{
// Only want to allow companion computer position control when

1
ArduPlane/GCS_Mavlink.h
View File

@ -67,6 +67,7 @@ private:
MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet);
void handle_set_position_target_global_int(const mavlink_message_t &msg);
void handle_set_position_target_local_ned(const mavlink_message_t &msg);
#if HAL_QUADPLANE_ENABLED
#if AP_MAVLINK_COMMAND_LONG_ENABLED