From b3257c0bf2942643ce3f4126742b50b5ce5a1d5d Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Fri, 8 Jan 2021 06:14:02 +0100 Subject: [PATCH] Support relative altitude for offboard global position setpoints Previously the coordinate_frame flag for setpoint_target_global_int message was not handled, resulting in the vehicle to not handle altitude correctly. This commit handles the relative altitude correctly. --- src/modules/mavlink/mavlink_receiver.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5e89b3a42..13893b036d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1085,8 +1085,17 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t pos_sp_triplet.current.position_valid = false; } else { + float target_altitude; + + if (set_position_target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + target_altitude = set_position_target_global_int.alt + local_pos.ref_alt; + + } else { + target_altitude = set_position_target_global_int.alt; //MAV_FRAME_GLOBAL_INT + } + globallocalconverter_tolocal(set_position_target_global_int.lat_int / 1e7, - set_position_target_global_int.lon_int / 1e7, set_position_target_global_int.alt, + set_position_target_global_int.lon_int / 1e7, target_altitude, &pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z); pos_sp_triplet.current.cruising_speed = get_offb_cruising_speed(); pos_sp_triplet.current.position_valid = true;