mirror of https://github.com/ArduPilot/ardupilot
GCS_MAVLink: factor out a virtual mav_frame_for_command_long method
This commit is contained in:
parent
7de20c09f2
commit
0b04f765d4
|
@ -649,6 +649,7 @@ protected:
|
|||
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);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
|
|
@ -4684,11 +4684,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_component_arm_disarm(const mavlink_comman
|
|||
return MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
|
||||
MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
|
||||
// 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
|
||||
{
|
||||
MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
if (command_long_stores_location((MAV_CMD)packet.command)) {
|
||||
// we must be able to supply a frame for the location:
|
||||
static const struct {
|
||||
uint32_t command;
|
||||
MAV_FRAME frame;
|
||||
|
@ -4700,16 +4700,23 @@ MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_lo
|
|||
};
|
||||
|
||||
// map from command to frame:
|
||||
bool found = false;
|
||||
for (const auto &map : frame_map) {
|
||||
if (map.command != packet.command) {
|
||||
if (map.command != packet_command) {
|
||||
continue;
|
||||
}
|
||||
frame = map.frame;
|
||||
found = true;
|
||||
break;
|
||||
return true;
|
||||
}
|
||||
if (!found) {
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
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;
|
||||
if (command_long_stores_location((MAV_CMD)packet.command)) {
|
||||
// we must be able to supply a frame for the location:
|
||||
if (!mav_frame_for_command_long(frame, (MAV_CMD)packet.command)) {
|
||||
return MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue