From 664b0ee0a9b705320a807e621094b5719f9f4fae Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Nov 2015 13:54:41 +0100 Subject: [PATCH] read setpoint type from type mask in offboard --- src/modules/mavlink/mavlink_receiver.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 31c1ef58d4..26bc3c0796 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -671,6 +671,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* yawrate ignore flag mapps to ignore_bodyrate */ offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); + + bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000); + bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000); + bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); + offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub == nullptr) { @@ -709,7 +714,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others + + if (is_takeoff_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + } else if(is_land_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + } else if(is_loiter_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + } else { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } /* set the local pos values */ if (!offboard_control_mode.ignore_position) {