From dfed3970d4a6010ffbfef61e047ff265cf81a998 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Apr 2025 14:18:04 +0200 Subject: [PATCH] navigator: use reference instead of pointer to pass the vehicle command to publish --- docs/en/camera/camera_architecture.md | 6 +- docs/en/camera/mavlink_v2_camera.md | 6 +- docs/ko/camera/camera_architecture.md | 6 +- docs/ko/camera/mavlink_v2_camera.md | 6 +- docs/uk/camera/camera_architecture.md | 6 +- docs/uk/camera/mavlink_v2_camera.md | 4 +- docs/zh/camera/camera_architecture.md | 6 +- docs/zh/camera/mavlink_v2_camera.md | 6 +- src/modules/navigator/land.cpp | 2 +- src/modules/navigator/mission_base.cpp | 2 +- src/modules/navigator/mission_block.cpp | 4 +- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 84 ++++++++++++------------ 13 files changed, 70 insertions(+), 70 deletions(-) diff --git a/docs/en/camera/camera_architecture.md b/docs/en/camera/camera_architecture.md index c5fc3894c6..6170c06224 100644 --- a/docs/en/camera/camera_architecture.md +++ b/docs/en/camera/camera_architecture.md @@ -88,9 +88,9 @@ Commands supported in missions, including camera commands, are shown in these me - Mission items are executed when set active. - `issue_command(_mission_item)` is called at the end of this to send the current non-waypoint command - [`MissionBlock::issue_command(const mission_item_s &item)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562) - - Creates a vehicle command for the mission item then calls `publish_vehicle_command` to publish it (`_navigator->publish_vehicle_command(&vehicle_command);`) - - [`void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) - - For some camera commands it sets the component ID to the camera component id (`vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA`) + - Creates a vehicle command for the mission item then calls `publish_vehicle_command` to publish it (`_navigator->publish_vehicle_command(vehicle_command);`) + - [`void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) + - For some camera commands it sets the component ID to the camera component id (`vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA`) - All others just get published to default component ID. - The `VehicleCommand` UORB topic is published. diff --git a/docs/en/camera/mavlink_v2_camera.md b/docs/en/camera/mavlink_v2_camera.md index c282c5041a..a53bb0df2a 100644 --- a/docs/en/camera/mavlink_v2_camera.md +++ b/docs/en/camera/mavlink_v2_camera.md @@ -83,11 +83,11 @@ void Mission::setActiveMissionItems() => https://github.com/PX4/PX4-Autopilot/bl Issuing command: MissionBlock::issue_command(const mission_item_s &item) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562 At end this publishes the current vehicle command - _navigator->publish_vehicle_command(&vehicle_command); + _navigator.publish_vehicle_command(vehicle_command); Publishing command: -void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 - For camera commands set to vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA +void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 + For camera commands set to vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA All others just get published as-is --> diff --git a/docs/ko/camera/camera_architecture.md b/docs/ko/camera/camera_architecture.md index 914cff3e38..026779fc50 100644 --- a/docs/ko/camera/camera_architecture.md +++ b/docs/ko/camera/camera_architecture.md @@ -88,9 +88,9 @@ Commands supported in missions, including camera commands, are shown in these me - Mission items are executed when set active. - `issue_command(_mission_item)` is called at the end of this to send the current non-waypoint command - [`MissionBlock::issue_command(const mission_item_s &item)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562) - - Creates a vehicle command for the mission item then calls `publish_vehicle_command` to publish it (`_navigator->publish_vehicle_command(&vehicle_command);`) - - [`void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) - - For some camera commands it sets the component ID to the camera component id (`vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA`) + - Creates a vehicle command for the mission item then calls `publish_vehicle_command` to publish it (`_navigator->publish_vehicle_command(vehicle_command);`) + - [`void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) + - For some camera commands it sets the component ID to the camera component id (`vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA`) - All others just get published to default component ID. - The `VehicleCommand` UORB topic is published. diff --git a/docs/ko/camera/mavlink_v2_camera.md b/docs/ko/camera/mavlink_v2_camera.md index ad27997a2f..72622459fa 100644 --- a/docs/ko/camera/mavlink_v2_camera.md +++ b/docs/ko/camera/mavlink_v2_camera.md @@ -83,11 +83,11 @@ void Mission::setActiveMissionItems() => https://github.com/PX4/PX4-Autopilot/bl Issuing command: MissionBlock::issue_command(const mission_item_s &item) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562 At end this publishes the current vehicle command - _navigator->publish_vehicle_command(&vcmd); + _navigator->publish_vehicle_command(vehicle_command); Publishing command: -void Navigator::publish_vehicle_command(vehicle_command_s *vcmd) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 - For camera commands set to vcmd->target_component = 100; // MAV_COMP_ID_CAMERA +void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 + For camera commands set to vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA All others just get published as-is --> diff --git a/docs/uk/camera/camera_architecture.md b/docs/uk/camera/camera_architecture.md index 49a4f602ca..f96eff55fd 100644 --- a/docs/uk/camera/camera_architecture.md +++ b/docs/uk/camera/camera_architecture.md @@ -88,9 +88,9 @@ PX4 повторно видає пункти камери, знайдені в - Предмети місії виконуються, коли вони активовані. - `issue_command(_mission_item)` викликається в кінці цього, щоб відправити поточну непунктову команду - [`MissionBlock::видача_команди(const mission_item_s &item)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562) - - Створює команду для місії транспортного засобу, а потім викликає `publish_vehicle_command` для публікації її (`_navigator->publish_vehicle_command(&vcmd);`) - - [`void Navigator::publish_vehicle_command(vehicle_command_s *vcmd)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) - - Для деяких команд камери це встановлює ідентифікатор компонента на ідентифікатор компонента камери (`vcmd->target_component = 100; // MAV_COMP_ID_CAMERA`) + - Створює команду для місії транспортного засобу, а потім викликає `publish_vehicle_command` для публікації її (`_navigator->publish_vehicle_command(vehicle_command);`) + - [`void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) + - Для деяких команд камери це встановлює ідентифікатор компонента на ідентифікатор компонента камери (`vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA`) - Усі інші просто публікуються під стандартний компонент ID. - Тема UORB `VehicleCommand` публікується. diff --git a/docs/uk/camera/mavlink_v2_camera.md b/docs/uk/camera/mavlink_v2_camera.md index 2ce1ab5f35..d1d2b5b8fa 100644 --- a/docs/uk/camera/mavlink_v2_camera.md +++ b/docs/uk/camera/mavlink_v2_camera.md @@ -86,8 +86,8 @@ MissionBlock::issue_command(const mission_item_s &item) => https://github.com/P _navigator->publish_vehicle_command(&vehicle_command); Publishing command: -void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 - For camera commands set to vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA +void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 + For camera commands set to vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA All others just get published as-is --> diff --git a/docs/zh/camera/camera_architecture.md b/docs/zh/camera/camera_architecture.md index e8ceee69e8..41375a5a3c 100644 --- a/docs/zh/camera/camera_architecture.md +++ b/docs/zh/camera/camera_architecture.md @@ -88,9 +88,9 @@ Commands supported in missions, including camera commands, are shown in these me - Mission items are executed when set active. - `issue_command(_mission_item)` is called at the end of this to send the current non-waypoint command - [`MissionBlock::issue_command(const mission_item_s &item)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562) - - Creates a vehicle command for the mission item then calls `publish_vehicle_command` to publish it (`_navigator->publish_vehicle_command(&vcmd);`) - - [`void Navigator::publish_vehicle_command(vehicle_command_s *vcmd)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) - - For some camera commands it sets the component ID to the camera component id (`vcmd->target_component = 100; // MAV_COMP_ID_CAMERA`) + - Creates a vehicle command for the mission item then calls `publish_vehicle_command` to publish it (`_navigator->publish_vehicle_command(vehicle_command);`) + - [`void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command)`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358) + - For some camera commands it sets the component ID to the camera component id (`vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA`) - All others just get published to default component ID. - The `VehicleCommand` UORB topic is published. diff --git a/docs/zh/camera/mavlink_v2_camera.md b/docs/zh/camera/mavlink_v2_camera.md index 3768eeefa7..6b58461b0b 100644 --- a/docs/zh/camera/mavlink_v2_camera.md +++ b/docs/zh/camera/mavlink_v2_camera.md @@ -83,11 +83,11 @@ void Mission::setActiveMissionItems() => https://github.com/PX4/PX4-Autopilot/bl Issuing command: MissionBlock::issue_command(const mission_item_s &item) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/mission_block.cpp#L543-L562 At end this publishes the current vehicle command - _navigator->publish_vehicle_command(&vehicle_command); + _navigator->publish_vehicle_command(vehicle_command); Publishing command: -void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 - For camera commands set to vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA +void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command) => https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp#L1358 + For camera commands set to vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA All others just get published as-is --> diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b0577f068e..55116c78e3 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -119,6 +119,6 @@ Land::on_active() // and thus always climb MIS_LND_ABRT_ALT vehicle_command.param7 = _navigator->get_global_position()->alt + _navigator->get_landing_abort_min_alt(); - _navigator->publish_vehicle_command(&vehicle_command); + _navigator->publish_vehicle_command(vehicle_command); } } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 7ad7b54bea..daac61e933 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -922,7 +922,7 @@ MissionBase::do_abort_landing() vehicle_command.param5 = _mission_item.lat; vehicle_command.param6 = _mission_item.lon; vehicle_command.param7 = alt_sp; - _navigator->publish_vehicle_command(&vehicle_command); + _navigator->publish_vehicle_command(vehicle_command); } void MissionBase::publish_navigator_mission_item() diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 719129dca7..3f2c621baa 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -549,7 +549,7 @@ MissionBlock::issue_command(const mission_item_s &item) } } - _navigator->publish_vehicle_command(&vehicle_command); + _navigator->publish_vehicle_command(vehicle_command); if (item_has_timeout(item)) { _timestamp_command_timeout = hrt_absolute_time(); @@ -785,7 +785,7 @@ MissionBlock::set_land_item(struct mission_item_s *item) vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION; vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; vehicle_command.param2 = 0.f; // normal unforced transition - _navigator->publish_vehicle_command(&vehicle_command); + _navigator->publish_vehicle_command(vehicle_command); } /* set the land item */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 5234dd49a4..30b9cd769a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -137,7 +137,7 @@ public: * * @param vehicle_command Vehicle command to publish */ - void publish_vehicle_command(vehicle_command_s *vehicle_command); + void publish_vehicle_command(vehicle_command_s &vehicle_command); #if CONFIG_NAVIGATOR_ADSB /** diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index eb4bc37f8e..dc53c6928d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -665,7 +665,7 @@ void Navigator::run() vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vehicle_command.param1 = _mission.get_land_start_index(); - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); } else { PX4_WARN("planned mission landing not available"); @@ -867,7 +867,7 @@ void Navigator::run() vehicle_command_s vehicle_command{}; vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION; vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t"); events::send(events::ID("navigator_transition_descend"), events::Log::Critical, "Transition to hover mode and descend"); @@ -1227,13 +1227,13 @@ void Navigator::take_traffic_conflict_action() case 2: { _rtl.set_return_alt_min(true); vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); break; } case 3: { vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); break; } @@ -1241,7 +1241,7 @@ void Navigator::take_traffic_conflict_action() case 4: { vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); break; } @@ -1392,32 +1392,32 @@ void Navigator::publish_navigator_status() } } -void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command) +void Navigator::publish_vehicle_command(vehicle_command_s &vehicle_command) { - vehicle_command->timestamp = hrt_absolute_time(); - vehicle_command->source_system = _vstatus.system_id; - vehicle_command->source_component = _vstatus.component_id; - vehicle_command->target_system = _vstatus.system_id; - vehicle_command->confirmation = false; - vehicle_command->from_external = false; + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.source_system = _vstatus.system_id; + vehicle_command.source_component = _vstatus.component_id; + vehicle_command.target_system = _vstatus.system_id; + vehicle_command.confirmation = false; + vehicle_command.from_external = false; int target_camera_component_id; // The camera commands are not processed on the autopilot but will be // sent to the mavlink links to other components. - switch (vehicle_command->command) { + switch (vehicle_command.command) { case NAV_CMD_IMAGE_START_CAPTURE: - if (static_cast(vehicle_command->param3) == 1) { + if (static_cast(vehicle_command.param3) == 1) { // When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this command - vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; - vehicle_command->param1 = 0.f; // Session control hide lens - vehicle_command->param2 = 0.f; // Zoom absolute position - vehicle_command->param3 = 0.f; // Zoom step - vehicle_command->param4 = 0.f; // Focus lock - vehicle_command->param5 = 1.; // Shoot command - vehicle_command->param6 = 0.; // Command identity - vehicle_command->param7 = 0.f; // Shot identifier + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; + vehicle_command.param1 = 0.f; // Session control hide lens + vehicle_command.param2 = 0.f; // Zoom absolute position + vehicle_command.param3 = 0.f; // Zoom step + vehicle_command.param4 = 0.f; // Focus lock + vehicle_command.param5 = 1.; // Shoot command + vehicle_command.param6 = 0.; // Command identity + vehicle_command.param7 = 0.f; // Shot identifier } else { // We are only capturing multiple if param3 is 0 or > 1. @@ -1425,65 +1425,65 @@ void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command) _is_capturing_images = true; } - target_camera_component_id = static_cast(vehicle_command->param1); // Target id from param 1 + target_camera_component_id = static_cast(vehicle_command.param1); // Target id from param 1 if (target_camera_component_id > 0 && target_camera_component_id < 256) { - vehicle_command->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA + vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA } break; case NAV_CMD_IMAGE_STOP_CAPTURE: _is_capturing_images = false; - target_camera_component_id = static_cast(vehicle_command->param1); // Target id from param 1 + target_camera_component_id = static_cast(vehicle_command.param1); // Target id from param 1 if (target_camera_component_id > 0 && target_camera_component_id < 256) { - vehicle_command->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA + vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA } break; case NAV_CMD_SET_CAMERA_MODE: - target_camera_component_id = static_cast(vehicle_command->param1); // Target id from param 1 + target_camera_component_id = static_cast(vehicle_command.param1); // Target id from param 1 if (target_camera_component_id > 0 && target_camera_component_id < 256) { - vehicle_command->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA + vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA } break; case NAV_CMD_SET_CAMERA_SOURCE: - target_camera_component_id = static_cast(vehicle_command->param1); // Target id from param 1 + target_camera_component_id = static_cast(vehicle_command.param1); // Target id from param 1 if (target_camera_component_id > 0 && target_camera_component_id < 256) { - vehicle_command->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA + vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA } break; case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: - vehicle_command->target_component = 100; // MAV_COMP_ID_CAMERA + vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA break; default: - vehicle_command->target_component = 0; + vehicle_command.target_component = 0; break; } - _vehicle_cmd_pub.publish(*vehicle_command); + _vehicle_cmd_pub.publish(vehicle_command); } void Navigator::publish_distance_sensor_mode_request() @@ -1535,7 +1535,7 @@ void Navigator::acquire_gimbal_control() vehicle_command.param2 = _vstatus.component_id; vehicle_command.param3 = -1.f; // Leave secondary control unchanged vehicle_command.param4 = -1.f; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); } void Navigator::release_gimbal_control() @@ -1546,7 +1546,7 @@ void Navigator::release_gimbal_control() vehicle_command.param2 = -3.f; vehicle_command.param3 = -1.f; // Leave secondary control unchanged vehicle_command.param4 = -1.f; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); } @@ -1557,7 +1557,7 @@ Navigator::stop_capturing_images() vehicle_command_s vehicle_command{}; vehicle_command.command = NAV_CMD_IMAGE_STOP_CAPTURE; vehicle_command.param1 = 0.f; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); // _is_capturing_images is reset inside publish_vehicle_command. } @@ -1610,7 +1610,7 @@ void Navigator::disable_camera_trigger() // Pause trigger vehicle_command.param1 = -1.f; vehicle_command.param3 = 1.f; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); } void Navigator::set_gimbal_neutral() @@ -1622,7 +1622,7 @@ void Navigator::set_gimbal_neutral() vehicle_command.param3 = NAN; // Don't set any angular velocities vehicle_command.param4 = NAN; vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; - publish_vehicle_command(&vehicle_command); + publish_vehicle_command(vehicle_command); } void Navigator::sendWarningDescentStoppedDueToTerrain()