From f402928888c95cd9109c4e16f8ea7de6cb8512a2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Apr 2025 11:00:04 +0200 Subject: [PATCH] navigator: Cleanup refactor of vehicle_command publishing Use reference instead of pointer and adhere to minimal conventions for clarity. These MAVLink dependent interfaces should be pushed out to the MAVLink module and internal interfaces be independent. A simple request like set gimbal to a neutral position takes three vehicle commands with a specific combination of float and double parameters which have to be filled with gimbal, vehicle and MAVLink context. This should really not be the case. --- src/modules/navigator/land.cpp | 17 ++- src/modules/navigator/mission_base.cpp | 18 ++- src/modules/navigator/mission_block.cpp | 36 +++--- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 158 +++++++++++------------ 5 files changed, 116 insertions(+), 121 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 2c351ffa6e..2e21910299 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -109,17 +109,16 @@ Land::on_active() if (_navigator->abort_landing()) { // send reposition cmd to get out of land mode (will loiter at current position and altitude) - vehicle_command_s vcmd = {}; - - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vcmd.param1 = -1; - vcmd.param2 = 1; - vcmd.param5 = _navigator->get_global_position()->lat; - vcmd.param6 = _navigator->get_global_position()->lon; + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vehicle_command.param1 = -1.f; + vehicle_command.param2 = 1.f; + vehicle_command.param5 = _navigator->get_global_position()->lat; + vehicle_command.param6 = _navigator->get_global_position()->lon; // as we don't know the landing point altitude assume the worst case (abort at 0m above ground), // and thus always climb MIS_LND_ABRT_ALT - vcmd.param7 = _navigator->get_global_position()->alt + _navigator->get_landing_abort_min_alt(); + vehicle_command.param7 = _navigator->get_global_position()->alt + _navigator->get_landing_abort_min_alt(); - _navigator->publish_vehicle_cmd(&vcmd); + _navigator->publish_vehicle_cmd(vehicle_command); } } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index c24a791f8d..8280397b4b 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -915,16 +915,14 @@ MissionBase::do_abort_landing() } // send reposition cmd to get out of mission - vehicle_command_s vcmd = {}; - - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vcmd.param1 = -1; - vcmd.param2 = 1; - vcmd.param5 = _mission_item.lat; - vcmd.param6 = _mission_item.lon; - vcmd.param7 = alt_sp; - - _navigator->publish_vehicle_cmd(&vcmd); + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vehicle_command.param1 = -1.f; + vehicle_command.param2 = 1.f; + vehicle_command.param5 = _mission_item.lat; + vehicle_command.param6 = _mission_item.lon; + vehicle_command.param7 = alt_sp; + _navigator->publish_vehicle_cmd(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 1b05628a6c..8807880a44 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -529,27 +529,27 @@ MissionBlock::issue_command(const mission_item_s &item) // Mission item's NAV_CMD enums directly map to the according vehicle command // So set the raw value directly (MAV_FRAME_MISSION mission item) - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - vcmd.param7 = item.params[6]; + vehicle_command_s vehicle_command{}; + vehicle_command.command = item.nav_cmd; + vehicle_command.param1 = item.params[0]; + vehicle_command.param2 = item.params[1]; + vehicle_command.param3 = item.params[2]; + vehicle_command.param4 = item.params[3]; + vehicle_command.param5 = static_cast(item.params[4]); + vehicle_command.param6 = static_cast(item.params[5]); + vehicle_command.param7 = item.params[6]; if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision - vcmd.param5 = item.lat; - vcmd.param6 = item.lon; + vehicle_command.param5 = item.lat; + vehicle_command.param6 = item.lon; if (item.altitude_is_relative) { - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; + vehicle_command.param7 = item.altitude + _navigator->get_home_position()->alt; } } - _navigator->publish_vehicle_cmd(&vcmd); + _navigator->publish_vehicle_cmd(vehicle_command); if (item_has_timeout(item)) { _timestamp_command_timeout = hrt_absolute_time(); @@ -781,11 +781,11 @@ MissionBlock::set_land_item(struct mission_item_s *item) /* VTOL transition to RW before landing */ if (_navigator->force_vtol()) { - vehicle_command_s vcmd = {}; - vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; - vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - vcmd.param2 = 0.0f; - _navigator->publish_vehicle_cmd(&vcmd); + vehicle_command_s vehicle_command{}; + vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION; + vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + vehicle_command.param2 = 0.f; + _navigator->publish_vehicle_cmd(vehicle_command); } /* set the land item */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 5e30d22ccc..313f732528 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -132,12 +132,12 @@ public: /** * @brief Publish a given specified vehicle command * - * Sets the target_component of the vehicle command accordingly depending on the - * vehicle command value (e.g. For Camera control, sets target system component id) + * Fill in timestamp, source and target IDs. + * target_component special handling (e.g. For Camera control, set camera ID) * - * @param vcmd Vehicle command to execute + * @param vcmd Vehicle command to publish */ - void publish_vehicle_cmd(vehicle_command_s *vcmd); + void publish_vehicle_cmd(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 34b4d2c00e..9c62f813d2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -662,10 +662,10 @@ void Navigator::run() uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; if (_mission.get_land_start_available()) { - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; - vcmd.param1 = _mission.get_land_start_index(); - publish_vehicle_cmd(&vcmd); + 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_cmd(vehicle_command); } else { PX4_WARN("planned mission landing not available"); @@ -864,10 +864,10 @@ void Navigator::run() if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND && _vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && force_vtol()) { - vehicle_command_s vcmd = {}; - vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; - vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - publish_vehicle_cmd(&vcmd); + 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_cmd(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"); @@ -1220,28 +1220,28 @@ void Navigator::load_fence_from_file(const char *filename) void Navigator::take_traffic_conflict_action() { - vehicle_command_s vcmd = {}; + vehicle_command_s vehicle_command{}; switch (_adsb_conflict._conflict_detection_params.traffic_avoidance_mode) { case 2: { _rtl.set_return_alt_min(true); - vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; - publish_vehicle_cmd(&vcmd); + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; + publish_vehicle_cmd(vehicle_command); break; } case 3: { - vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - publish_vehicle_cmd(&vcmd); + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; + publish_vehicle_cmd(vehicle_command); break; } case 4: { - vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; - publish_vehicle_cmd(&vcmd); + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; + publish_vehicle_cmd(vehicle_command); break; } @@ -1392,34 +1392,32 @@ void Navigator::publish_navigator_status() } } -void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) +void Navigator::publish_vehicle_cmd(vehicle_command_s &vehicle_command) { - vcmd->timestamp = hrt_absolute_time(); - vcmd->source_system = _vstatus.system_id; - vcmd->source_component = _vstatus.component_id; - vcmd->target_system = _vstatus.system_id; - vcmd->confirmation = false; - vcmd->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 (vcmd->command) { - - + switch (vehicle_command.command) { case NAV_CMD_IMAGE_START_CAPTURE: - if (static_cast(vcmd->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 cmd - vcmd->param1 = 0.0f; - vcmd->param2 = 0.0f; - vcmd->param3 = 0.0f; - vcmd->param4 = 0.0f; - vcmd->param5 = 1.0; - vcmd->param6 = 0.0; - vcmd->param7 = 0.0f; - vcmd->command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; + vehicle_command.param1 = 0.f; + vehicle_command.param2 = 0.f; + vehicle_command.param3 = 0.f; + vehicle_command.param4 = 0.f; + vehicle_command.param5 = 1.; + vehicle_command.param6 = 0.; + vehicle_command.param7 = 0.f; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; } else { // We are only capturing multiple if param3 is 0 or > 1. @@ -1427,65 +1425,65 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) _is_capturing_images = true; } - target_camera_component_id = static_cast(vcmd->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) { - vcmd->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vcmd->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(vcmd->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) { - vcmd->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vcmd->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(vcmd->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) { - vcmd->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vcmd->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(vcmd->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) { - vcmd->target_component = target_camera_component_id; + vehicle_command.target_component = target_camera_component_id; } else { - vcmd->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: - vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA break; default: - vcmd->target_component = 0; + vehicle_command.target_component = 0; break; } - _vehicle_cmd_pub.publish(*vcmd); + _vehicle_cmd_pub.publish(vehicle_command); } void Navigator::publish_distance_sensor_mode_request() @@ -1531,24 +1529,24 @@ void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_ void Navigator::acquire_gimbal_control() { - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vcmd.param1 = _vstatus.system_id; - vcmd.param2 = _vstatus.component_id; - vcmd.param3 = -1.0f; // Leave unchanged. - vcmd.param4 = -1.0f; // Leave unchanged. - publish_vehicle_cmd(&vcmd); + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _vstatus.system_id; // Take primary control + vehicle_command.param2 = _vstatus.component_id; + vehicle_command.param3 = -1.f; // Leave secondary control unchanged + vehicle_command.param4 = -1.f; + publish_vehicle_cmd(vehicle_command); } void Navigator::release_gimbal_control() { - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vcmd.param1 = -3.0f; // Remove control if it had it. - vcmd.param2 = -3.0f; // Remove control if it had it. - vcmd.param3 = -1.0f; // Leave unchanged. - vcmd.param4 = -1.0f; // Leave unchanged. - publish_vehicle_cmd(&vcmd); + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = -3.f; // Remove primary control if it was taken + vehicle_command.param2 = -3.f; + vehicle_command.param3 = -1.f; // Leave secondary control unchanged + vehicle_command.param4 = -1.f; + publish_vehicle_cmd(vehicle_command); } @@ -1556,10 +1554,10 @@ void Navigator::stop_capturing_images() { if (_is_capturing_images) { - vehicle_command_s vcmd = {}; - vcmd.command = NAV_CMD_IMAGE_STOP_CAPTURE; - vcmd.param1 = 0.0f; - publish_vehicle_cmd(&vcmd); + vehicle_command_s vehicle_command{}; + vehicle_command.command = NAV_CMD_IMAGE_STOP_CAPTURE; + vehicle_command.param1 = 0.f; + publish_vehicle_cmd(vehicle_command); // _is_capturing_images is reset inside publish_vehicle_cmd. } @@ -1607,24 +1605,24 @@ void Navigator::mode_completed(uint8_t nav_state, uint8_t result) void Navigator::disable_camera_trigger() { // Disable camera trigger - vehicle_command_s cmd {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; // Pause trigger - cmd.param1 = -1.0f; - cmd.param3 = 1.0f; - publish_vehicle_cmd(&cmd); + vehicle_command.param1 = -1.f; + vehicle_command.param3 = 1.f; + publish_vehicle_cmd(vehicle_command); } void Navigator::set_gimbal_neutral() { - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; - vcmd.param1 = NAN; - vcmd.param2 = NAN; - vcmd.param3 = NAN; - vcmd.param4 = NAN; - vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; - publish_vehicle_cmd(&vcmd); + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; + vehicle_command.param1 = NAN; // Don't set any angles + vehicle_command.param2 = NAN; + 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_cmd(vehicle_command); } void Navigator::sendWarningDescentStoppedDueToTerrain()