diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index b147bef092..4bce9173d7 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -165,7 +165,10 @@ int8 ARMING_ACTION_ARM = 1 uint8 GRIPPER_ACTION_RELEASE = 0 uint8 GRIPPER_ACTION_GRAB = 1 -uint8 ORB_QUEUE_LENGTH = 8 +uint8 frame # The coordinate system of the COMMAND. +uint8 FRAME_UNKNOWN = 0 # Not specified. +uint8 FRAME_GLOBAL = 1 # Global (WGS84) coordinate frame + MSL altitude. +uint8 FRAME_GLOBAL_RELATIVE_ALT = 2 # Global (WGS84) coordinate frame + altitude relative to the home position. float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum. @@ -184,4 +187,6 @@ bool from_external uint16 COMPONENT_MODE_EXECUTOR_START = 1000 +uint8 ORB_QUEUE_LENGTH = 8 + # TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0385c6a1eb..7667506d9a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -469,6 +469,24 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) vcmd.param3 = cmd_mavlink.param3; vcmd.param4 = cmd_mavlink.param4; + switch (cmd_mavlink.frame) { + case MAV_FRAME_GLOBAL: + + // FALLTHROUGH + case MAV_FRAME_GLOBAL_INT: + vcmd.frame = vehicle_command_s::FRAME_GLOBAL; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + vcmd.frame = vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALT; + break; + + default: + vcmd.frame = vehicle_command_s::FRAME_UNKNOWN; + PX4_WARN("invalid frame %" PRIu8 " for command %" PRIu16, cmd_mavlink.frame, cmd_mavlink.command); + break; + } + if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) { // INT32_MAX for x and y means to ignore it. vcmd.param5 = (double)NAN; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b7..717090bf0a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -108,6 +108,7 @@ Land::on_active() vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.frame = vehicle_command_s::FRAME_GLOBAL; vcmd.param1 = -1; vcmd.param2 = 1; vcmd.param5 = _navigator->get_global_position()->lat; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 595f548c83..8178203aee 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -855,6 +855,7 @@ MissionBase::do_abort_landing() vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.frame = vehicle_command_s::FRAME_GLOBAL; vcmd.param1 = -1; vcmd.param2 = 1; vcmd.param5 = _mission_item.lat; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d5ea7ac22d..280a84b92f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -267,6 +267,7 @@ void Navigator::run() vehicle_global_position_s position_setpoint{}; + // latitude/longitude if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { position_setpoint.lat = cmd.param5; position_setpoint.lon = cmd.param6; @@ -276,7 +277,24 @@ void Navigator::run() position_setpoint.lon = get_global_position()->lon; } - position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; + // altitude + if (PX4_ISFINITE(cmd.param7)) { + switch (cmd.frame) { + default: + + // FALLTHROUGH + case vehicle_command_s::FRAME_GLOBAL: + position_setpoint.alt = cmd.param7; + break; + + case vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALT: + position_setpoint.alt = cmd.param7 + get_home_position()->alt; + break; + } + + } else { + position_setpoint.alt = get_global_position()->alt; + } if (geofence_allows_position(position_setpoint)) { position_setpoint_triplet_s *rep = get_reposition_triplet();