Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: MissionItemProtocol_Rally: deal with alt frame

Browse Source
This commit is contained in:
Iampete1 2023-10-25 01:58:32 +01:00 committed by Andrew Tridgell
parent 8f130f962e
commit d25d1c253e
1 changed files with 49 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
View File

@ -26,6 +26,7 @@
#include <AP_Logger/AP_Logger.h>
#include <AP_Rally/AP_Rally.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Terrain/AP_Terrain.h>
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;