From fc7c7ac20630951b17e8644ccc175b9c4e743409 Mon Sep 17 00:00:00 2001 From: Gabriel Moreno Date: Thu, 28 Feb 2019 14:03:21 -0500 Subject: [PATCH] simulator: fix to set system and component IDs as defined in params The simulator had hardcoded component ID and system ID (sysID was 0), ignoring what was set up in the params MAV_SYS_ID and MAV_COMP_ID. This caused an issue with multi-vehicle simulations that that rely on sysID to identify the vehicles. Signed-off-by: Gabriel Moreno --- src/modules/simulator/simulator.h | 4 +++- src/modules/simulator/simulator_mavlink.cpp | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 96cc2560f0ea..817c87e132ad 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -378,7 +378,9 @@ class Simulator : public ModuleParams DEFINE_PARAMETERS( (ParamFloat) _battery_drain_interval_s, ///< battery drain interval - (ParamInt) _param_system_type + (ParamInt) _param_system_type, + (ParamInt) _param_system_id, + (ParamInt) _param_component_id ) #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 4570dcd3cedc..f34f289c9aec 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -184,7 +184,7 @@ void Simulator::send_controls() const mavlink_hil_actuator_controls_t hil_act_control = actuator_controls_from_outputs(actuators); mavlink_message_t message{}; - mavlink_msg_hil_actuator_controls_encode(0, 200, &message, &hil_act_control); + mavlink_msg_hil_actuator_controls_encode(_param_system_id.get(), _param_component_id.get(), &message, &hil_act_control); PX4_DEBUG("sending controls t=%ld (%ld)", actuators.timestamp, hil_act_control.time_usec); @@ -618,7 +618,7 @@ void Simulator::request_hil_state_quaternion() cmd_long.command = MAV_CMD_SET_MESSAGE_INTERVAL; cmd_long.param1 = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; cmd_long.param2 = 5e3; - mavlink_msg_command_long_encode(0, 50, &message, &cmd_long); + mavlink_msg_command_long_encode(_param_system_id.get(), _param_component_id.get(), &message, &cmd_long); send_mavlink_message(message); } @@ -628,7 +628,7 @@ void Simulator::send_heartbeat() mavlink_message_t message = {}; hb.autopilot = 12; hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; - mavlink_msg_heartbeat_encode(0, 50, &message, &hb); + mavlink_msg_heartbeat_encode(_param_system_id.get(), _param_component_id.get(), &message, &hb); send_mavlink_message(message); }