Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: support sysid targetting

Browse Source
This commit is contained in:
Peter Barker 2019-03-16 18:07:36 +11:00 committed by Randy Mackay
parent c649fd1468
commit a893a5483c
2 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -401,6 +401,9 @@ protected:
MAV_RESULT handle_command_battery_reset(const mavlink_command_long_t &packet);
void handle_command_long(const mavlink_message_t &msg);
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_roi_sysid(const uint8_t sysid);
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet);

29
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1294,6 +1294,9 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
// the routing code has indicated we should not handle this packet locally
return;
}
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
handle_mount_message(msg);
}
if (!accept_packet(status, msg)) {
// e.g. enforce-sysid says we shouldn't look at this packet
return;
@ -3628,6 +3631,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
break;
}
case MAV_CMD_DO_SET_ROI_SYSID:
return handle_command_do_set_roi_sysid(packet);
case MAV_CMD_DO_SET_ROI_LOCATION:
case MAV_CMD_DO_SET_ROI:
result = handle_command_do_set_roi(packet);
@ -3835,6 +3841,27 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_do_set_home(const mavlink_command_int
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi_sysid(const uint8_t sysid)
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
mount->set_target_sysid(sysid);
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet)
{
return handle_command_do_set_roi_sysid((uint8_t)packet.param1);
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet)
{
return handle_command_do_set_roi_sysid((uint8_t)packet.param1);
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_int_t &packet)
{
// be aware that this method is called for both MAV_CMD_DO_SET_ROI
@ -3887,6 +3914,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_SET_ROI_LOCATION:
return handle_command_do_set_roi(packet);
case MAV_CMD_DO_SET_ROI_SYSID:
return handle_command_do_set_roi_sysid(packet);
case MAV_CMD_DO_SET_HOME:
return handle_command_int_do_set_home(packet);
default: