From f3eb18884e2573bc9245fdec9f5d0d9420ba8254 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 13:51:29 +0800 Subject: [PATCH 1/2] MAVLink: use the new MAVLink GUIDED HEADING_TYPE_DEFAULT --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index 83e75ffdb2709..08da786b2d94c 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 83e75ffdb2709f4821b9746477639e2ae6df103f +Subproject commit 08da786b2d94ce0955a82f92ab2166c347259623 From 594465a461a414a22ab92dadf254a44b337fa7fe Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 13:58:36 +0800 Subject: [PATCH 2/2] ArduPlane: use the new MAVLink GUIDED HEADING_TYPE_DEFAULT --- ArduPlane/GCS_Mavlink.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d2800c367631f..9256fdfe01cb3 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -863,6 +863,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); +#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; +#endif // add home alt if needed if (requested_position.relative_alt) { @@ -973,14 +976,20 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); - // course over ground - if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int + switch(HEADING_TYPE(packet.param1)) { + case HEADING_TYPE_COURSE_OVER_GROUND: + // course over ground plane.guided_state.target_heading_type = GUIDED_HEADING_COG; plane.prev_WP_loc = plane.current_loc; - // normal vehicle heading - } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int + break; + case HEADING_TYPE_HEADING: + // normal vehicle heading plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING; - } else { + break; + case HEADING_TYPE_DEFAULT: + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + return MAV_RESULT_ACCEPTED; + default: // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). return MAV_RESULT_DENIED; }