aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: add support for MAV_CMD_DEBUG_TRAP

Browse Source
This commit is contained in:
Peter Barker 2019-02-08 19:41:28 +11:00 committed by Peter Barker
parent 56044b8b04
commit 1650979a2b
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -440,6 +440,7 @@ protected:
MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_debug_trap(const mavlink_command_long_t &packet);
void handle_optical_flow(const mavlink_message_t &msg);

16
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3608,6 +3608,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_l
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_long_t &packet)
{
// magic number must be supplied to trap; you must *really* mean it.
if (uint32_t(packet.param1) != 32451) {
return MAV_RESULT_DENIED;
}
if (hal.util->trap()) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_UNSUPPORTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_long_t &packet)
{
AP_Gripper *gripper = AP::gripper();
@ -3775,6 +3787,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_get_home_position(packet);
break;
case MAV_CMD_DEBUG_TRAP:
result = handle_command_debug_trap(packet);
break;
case MAV_CMD_PREFLIGHT_STORAGE:
if (is_equal(packet.param1, 2.0f)) {
AP_Param::erase_all();