diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp index 8ba19415bacbe..8c9271884537c 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp @@ -26,6 +26,7 @@ #include #include #include +#include MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd) { @@ -57,10 +58,6 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyL if (cmd.command != MAV_CMD_NAV_RALLY_POINT) { return MAV_MISSION_UNSUPPORTED; } - if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && - cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) { - return MAV_MISSION_UNSUPPORTED_FRAME; - } if (!check_lat(cmd.x)) { return MAV_MISSION_INVALID_PARAM5_X; } @@ -71,6 +68,32 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyL return MAV_MISSION_INVALID_PARAM7; } ret = {}; + + switch (cmd.frame) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_INT: + ret.alt_frame = uint8_t(Location::AltFrame::ABSOLUTE); + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + ret.alt_frame = uint8_t(Location::AltFrame::ABOVE_HOME); + break; + +#if AP_TERRAIN_AVAILABLE + case MAV_FRAME_GLOBAL_TERRAIN_ALT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + ret.alt_frame = uint8_t(Location::AltFrame::ABOVE_TERRAIN); + break; +#endif + + default: + return MAV_MISSION_UNSUPPORTED_FRAME; + } + + // Fresh points always use new alt frame format + ret.alt_frame_valid = true; + ret.lat = cmd.x; ret.lng = cmd.y; ret.alt = cmd.z; @@ -91,7 +114,29 @@ bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq, return false; } + // Default to relative to home ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + + if (rallypoint.alt_frame_valid == 1) { + switch (Location::AltFrame(rallypoint.alt_frame)) { + case Location::AltFrame::ABSOLUTE: + ret_packet.frame = MAV_FRAME_GLOBAL; + break; + + case Location::AltFrame::ABOVE_HOME: + ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + break; + + case Location::AltFrame::ABOVE_ORIGIN: + // Above origin alt frame is not supported + return false; + + case Location::AltFrame::ABOVE_TERRAIN: + ret_packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT; + break; + } + } + ret_packet.command = MAV_CMD_NAV_RALLY_POINT; ret_packet.x = rallypoint.lat; ret_packet.y = rallypoint.lng;