Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: factor out a virtual mav_frame_for_command_long method

This commit is contained in:
Peter Barker 2023-10-17 13:19:59 +11:00 committed by Peter Barker
parent 7de20c09f2
commit 0b04f765d4
2 changed files with 29 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -649,6 +649,7 @@ protected:
MAV_RESULT handle_command_mag_cal(const mavlink_command_int_t &packet); MAV_RESULT handle_command_mag_cal(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet); MAV_RESULT handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet);
virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const;
MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg); MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg); virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet); MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);

49
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4684,32 +4684,39 @@ MAV_RESULT GCS_MAVLINK::handle_command_component_arm_disarm(const mavlink_comman
return MAV_RESULT_UNSUPPORTED; return MAV_RESULT_UNSUPPORTED;
} }
// when conveyed via COMMAND_LONG, a command doesn't come with an
// explicit frame. When conveying a location they do have an assumed
// frame in ArduPilot, and this function returns that frame.
bool GCS_MAVLINK::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const
{
static const struct {
uint32_t command;
MAV_FRAME frame;
} frame_map[] {
{ MAV_CMD_FIXED_MAG_CAL_YAW, MAV_FRAME_GLOBAL_RELATIVE_ALT },
{ MAV_CMD_DO_SET_ROI, MAV_FRAME_GLOBAL_RELATIVE_ALT },
{ MAV_CMD_DO_SET_ROI_LOCATION, MAV_FRAME_GLOBAL_RELATIVE_ALT },
{ MAV_CMD_DO_SET_HOME, MAV_FRAME_GLOBAL },
};
// map from command to frame:
for (const auto &map : frame_map) {
if (map.command != packet_command) {
continue;
}
frame = map.frame;
return true;
}
return false;
}
MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg) MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{ {
MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
if (command_long_stores_location((MAV_CMD)packet.command)) { if (command_long_stores_location((MAV_CMD)packet.command)) {
// we must be able to supply a frame for the location: // we must be able to supply a frame for the location:
static const struct { if (!mav_frame_for_command_long(frame, (MAV_CMD)packet.command)) {
uint32_t command;
MAV_FRAME frame;
} frame_map[] {
{ MAV_CMD_FIXED_MAG_CAL_YAW, MAV_FRAME_GLOBAL_RELATIVE_ALT },
{ MAV_CMD_DO_SET_ROI, MAV_FRAME_GLOBAL_RELATIVE_ALT },
{ MAV_CMD_DO_SET_ROI_LOCATION, MAV_FRAME_GLOBAL_RELATIVE_ALT },
{ MAV_CMD_DO_SET_HOME, MAV_FRAME_GLOBAL },
};
// map from command to frame:
bool found = false;
for (const auto &map : frame_map) {
if (map.command != packet.command) {
continue;
}
frame = map.frame;
found = true;
break;
}
if (!found) {
return MAV_RESULT_UNSUPPORTED; return MAV_RESULT_UNSUPPORTED;
} }
} }