forked from Archive/PX4-Autopilot
vmount mavlink input: process commands only if the target matches our sys & comp id
This commit is contained in:
parent
fd05c09447
commit
923cdbcbfb
|
@ -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];
|
||||
}
|
||||
|
|
|
@ -42,6 +42,7 @@
|
|||
#include "input.h"
|
||||
#include "input_rc.h"
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue