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 MAV_CMD_SET_EKF_SOURCE_SET command

Browse Source 
this allows external systems to set the active EKF source set
This commit is contained in:
Randy Mackay 2021-08-14 15:43:44 +09:00 committed by Andrew Tridgell
parent d92c5589a9
commit 2fba31332b
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

@ -505,6 +505,7 @@ protected:
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);
MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_long_t &packet);
void handle_optical_flow(const mavlink_message_t &msg);

16
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3945,6 +3945,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_long_t &
return MAV_RESULT_UNSUPPORTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_long_t &packet)
{
// source set must be between 1 and 3
uint32_t source_set = uint32_t(packet.param1);
if ((source_set >= 1) && (source_set <= 3)) {
// mavlink command uses range 1 to 3 while ahrs interface accepts 0 to 2
AP::ahrs().set_posvelyaw_source_set(source_set-1);
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_DENIED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_long_t &packet)
{
AP_Gripper *gripper = AP::gripper();
@ -4181,6 +4193,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_debug_trap(packet);
break;
case MAV_CMD_SET_EKF_SOURCE_SET:
result = handle_command_set_ekf_source_set(packet);
break;
case MAV_CMD_PREFLIGHT_STORAGE:
if (is_equal(packet.param1, 2.0f)) {
AP_Param::erase_all();