do not pass executor id if not in charge yet

Auterion · Jun 18, 2024 · 524354c · 524354c
1 parent cc78c85
commit 524354c
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/px4_ros2_cpp/src/components/mode_executor.cpp b/px4_ros2_cpp/src/components/mode_executor.cpp
@@ -118,7 +118,11 @@ Result ModeExecutorBase::sendCommandSync(
   cmd.param5 = param5;
   cmd.param6 = param6;
   cmd.param7 = param7;
-  cmd.source_component = px4_msgs::msg::VehicleCommand::COMPONENT_MODE_EXECUTOR_START + id();
+  if(!_is_in_charge){
+    cmd.source_component = px4_msgs::msg::VehicleCommand::COMPONENT_MODE_EXECUTOR_START;
+  }else{
+    cmd.source_component = px4_msgs::msg::VehicleCommand::COMPONENT_MODE_EXECUTOR_START + id();
+  } 
   cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;