diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index 94333f443e..30ebe3c62e 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -177,6 +177,23 @@ void InputMavlinkROI::print_status() InputMavlinkCmdMount::InputMavlinkCmdMount() { + param_t handle = param_find("MAV_SYS_ID"); + + if (handle == PARAM_INVALID) { + _mav_sys_id = 1; + + } else { + param_get(handle, &_mav_sys_id); + } + + handle = param_find("MAV_COMP_ID"); + + if (handle == PARAM_INVALID) { + _mav_comp_id = 1; + + } else { + param_get(handle, &_mav_comp_id); + } } InputMavlinkCmdMount::~InputMavlinkCmdMount() @@ -220,6 +237,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con vehicle_command_s vehicle_command; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + // process only if the command is for us + if (vehicle_command.target_system != _mav_sys_id || vehicle_command.target_component != _mav_comp_id) { + return 0; + } + for (int i = 0; i < 3; ++i) { _control_data.stabilize_axis[i] = _stabilize[i]; } diff --git a/src/drivers/vmount/input_mavlink.h b/src/drivers/vmount/input_mavlink.h index 30f2fb5b13..d271fad588 100644 --- a/src/drivers/vmount/input_mavlink.h +++ b/src/drivers/vmount/input_mavlink.h @@ -42,6 +42,7 @@ #include "input.h" #include "input_rc.h" #include +#include namespace vmount { @@ -94,6 +95,9 @@ private: int _vehicle_command_sub = -1; orb_advert_t _vehicle_command_ack_pub = nullptr; bool _stabilize[3] = { false, false, false }; + + int32_t _mav_sys_id; ///< our mavlink system id + int32_t _mav_comp_id; ///< our mavlink component id };