fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 14:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: Cleanup refactor of vehicle_command publishing

Browse Source 
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.
This commit is contained in:
Matthias Grob
2025-04-25 11:00:04 +02:00
parent 5c2a898edd
commit f402928888
5 changed files with 116 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/land.cpp
+8 -9
View File
@@ -109,17 +109,16 @@ Land::on_active()
if (_navigator->abort_landing()) { if (_navigator->abort_landing()) {
// send reposition cmd to get out of land mode (will loiter at current position and altitude) // send reposition cmd to get out of land mode (will loiter at current position and altitude)
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; vehicle_command.param1 = -1.f;
vcmd.param1 = -1; vehicle_command.param2 = 1.f;
vcmd.param2 = 1; vehicle_command.param5 = _navigator->get_global_position()->lat;
vcmd.param5 = _navigator->get_global_position()->lat; vehicle_command.param6 = _navigator->get_global_position()->lon;
vcmd.param6 = _navigator->get_global_position()->lon;
// as we don't know the landing point altitude assume the worst case (abort at 0m above ground), // 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 // 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);
} }
} }
src/modules/navigator/mission_base.cpp
+8 -10
View File
@@ -915,16 +915,14 @@ MissionBase::do_abort_landing()
} }
// send reposition cmd to get out of mission // send reposition cmd to get out of mission
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; vehicle_command.param1 = -1.f;
vcmd.param1 = -1; vehicle_command.param2 = 1.f;
vcmd.param2 = 1; vehicle_command.param5 = _mission_item.lat;
vcmd.param5 = _mission_item.lat; vehicle_command.param6 = _mission_item.lon;
vcmd.param6 = _mission_item.lon; vehicle_command.param7 = alt_sp;
vcmd.param7 = alt_sp; _navigator->publish_vehicle_cmd(vehicle_command);
_navigator->publish_vehicle_cmd(&vcmd);
} }
void MissionBase::publish_navigator_mission_item() void MissionBase::publish_navigator_mission_item()
src/modules/navigator/mission_block.cpp
+18 -18
View File
@@ -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 // Mission item's NAV_CMD enums directly map to the according vehicle command
// So set the raw value directly (MAV_FRAME_MISSION mission item) // So set the raw value directly (MAV_FRAME_MISSION mission item)
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = item.nav_cmd; vehicle_command.command = item.nav_cmd;
vcmd.param1 = item.params[0]; vehicle_command.param1 = item.params[0];
vcmd.param2 = item.params[1]; vehicle_command.param2 = item.params[1];
vcmd.param3 = item.params[2]; vehicle_command.param3 = item.params[2];
vcmd.param4 = item.params[3]; vehicle_command.param4 = item.params[3];
vcmd.param5 = static_cast<double>(item.params[4]); vehicle_command.param5 = static_cast<double>(item.params[4]);
vcmd.param6 = static_cast<double>(item.params[5]); vehicle_command.param6 = static_cast<double>(item.params[5]);
vcmd.param7 = item.params[6]; vehicle_command.param7 = item.params[6];
if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { 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 // 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; vehicle_command.param5 = item.lat;
vcmd.param6 = item.lon; vehicle_command.param6 = item.lon;
if (item.altitude_is_relative) { 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)) { if (item_has_timeout(item)) {
_timestamp_command_timeout = hrt_absolute_time(); _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 */ /* VTOL transition to RW before landing */
if (_navigator->force_vtol()) { if (_navigator->force_vtol()) {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION;
vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
vcmd.param2 = 0.0f; vehicle_command.param2 = 0.f;
_navigator->publish_vehicle_cmd(&vcmd); _navigator->publish_vehicle_cmd(vehicle_command);
} }
/* set the land item */ /* set the land item */
src/modules/navigator/navigator.h
+4 -4
View File
@@ -132,12 +132,12 @@ public:
/** /**
* @brief Publish a given specified vehicle command * @brief Publish a given specified vehicle command
* *
* Sets the target_component of the vehicle command accordingly depending on the * Fill in timestamp, source and target IDs.
* vehicle command value (e.g. For Camera control, sets target system component id) * 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 #if CONFIG_NAVIGATOR_ADSB
/** /**
src/modules/navigator/navigator_main.cpp
+78 -80
View File
@@ -662,10 +662,10 @@ void Navigator::run()
uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
if (_mission.get_land_start_available()) { if (_mission.get_land_start_available()) {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
vcmd.param1 = _mission.get_land_start_index(); vehicle_command.param1 = _mission.get_land_start_index();
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
} else { } else {
PX4_WARN("planned mission landing not available"); PX4_WARN("planned mission landing not available");
@@ -864,10 +864,10 @@ void Navigator::run()
if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND && if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND &&
_vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && _vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
force_vtol()) { force_vtol()) {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION;
vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t"); mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t");
events::send(events::ID("navigator_transition_descend"), events::Log::Critical, events::send(events::ID("navigator_transition_descend"), events::Log::Critical,
"Transition to hover mode and descend"); "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() void Navigator::take_traffic_conflict_action()
{ {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
switch (_adsb_conflict._conflict_detection_params.traffic_avoidance_mode) { switch (_adsb_conflict._conflict_detection_params.traffic_avoidance_mode) {
case 2: { case 2: {
_rtl.set_return_alt_min(true); _rtl.set_return_alt_min(true);
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
break; break;
} }
case 3: { case 3: {
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
break; break;
} }
case 4: { case 4: {
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
break; 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(); vehicle_command.timestamp = hrt_absolute_time();
vcmd->source_system = _vstatus.system_id; vehicle_command.source_system = _vstatus.system_id;
vcmd->source_component = _vstatus.component_id; vehicle_command.source_component = _vstatus.component_id;
vcmd->target_system = _vstatus.system_id; vehicle_command.target_system = _vstatus.system_id;
vcmd->confirmation = false; vehicle_command.confirmation = false;
vcmd->from_external = false; vehicle_command.from_external = false;
int target_camera_component_id; int target_camera_component_id;
// The camera commands are not processed on the autopilot but will be // The camera commands are not processed on the autopilot but will be
// sent to the mavlink links to other components. // sent to the mavlink links to other components.
switch (vcmd->command) { switch (vehicle_command.command) {
case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_START_CAPTURE:
if (static_cast<int>(vcmd->param3) == 1) { if (static_cast<int>(vehicle_command.param3) == 1) {
// When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this cmd // When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this cmd
vcmd->param1 = 0.0f; vehicle_command.param1 = 0.f;
vcmd->param2 = 0.0f; vehicle_command.param2 = 0.f;
vcmd->param3 = 0.0f; vehicle_command.param3 = 0.f;
vcmd->param4 = 0.0f; vehicle_command.param4 = 0.f;
vcmd->param5 = 1.0; vehicle_command.param5 = 1.;
vcmd->param6 = 0.0; vehicle_command.param6 = 0.;
vcmd->param7 = 0.0f; vehicle_command.param7 = 0.f;
vcmd->command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
} else { } else {
// We are only capturing multiple if param3 is 0 or > 1. // 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; _is_capturing_images = true;
} }
target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1 target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
if (target_camera_component_id > 0 && target_camera_component_id < 256) { 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 { } else {
vcmd->target_component = 100; // MAV_COMP_ID_CAMERA vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
} }
break; break;
case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE:
_is_capturing_images = false; _is_capturing_images = false;
target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1 target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
if (target_camera_component_id > 0 && target_camera_component_id < 256) { 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 { } else {
vcmd->target_component = 100; // MAV_COMP_ID_CAMERA vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
} }
break; break;
case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_MODE:
target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1 target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
if (target_camera_component_id > 0 && target_camera_component_id < 256) { 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 { } else {
vcmd->target_component = 100; // MAV_COMP_ID_CAMERA vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
} }
break; break;
case NAV_CMD_SET_CAMERA_SOURCE: case NAV_CMD_SET_CAMERA_SOURCE:
target_camera_component_id = static_cast<int>(vcmd->param1); // Target id from param 1 target_camera_component_id = static_cast<int>(vehicle_command.param1); // Target id from param 1
if (target_camera_component_id > 0 && target_camera_component_id < 256) { 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 { } else {
vcmd->target_component = 100; // MAV_COMP_ID_CAMERA vehicle_command.target_component = 100; // MAV_COMP_ID_CAMERA
} }
break; break;
case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_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; break;
default: default:
vcmd->target_component = 0; vehicle_command.target_component = 0;
break; break;
} }
_vehicle_cmd_pub.publish(*vcmd); _vehicle_cmd_pub.publish(vehicle_command);
} }
void Navigator::publish_distance_sensor_mode_request() 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() void Navigator::acquire_gimbal_control()
{ {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vcmd.param1 = _vstatus.system_id; vehicle_command.param1 = _vstatus.system_id; // Take primary control
vcmd.param2 = _vstatus.component_id; vehicle_command.param2 = _vstatus.component_id;
vcmd.param3 = -1.0f; // Leave unchanged. vehicle_command.param3 = -1.f; // Leave secondary control unchanged
vcmd.param4 = -1.0f; // Leave unchanged. vehicle_command.param4 = -1.f;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
} }
void Navigator::release_gimbal_control() void Navigator::release_gimbal_control()
{ {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vcmd.param1 = -3.0f; // Remove control if it had it. vehicle_command.param1 = -3.f; // Remove primary control if it was taken
vcmd.param2 = -3.0f; // Remove control if it had it. vehicle_command.param2 = -3.f;
vcmd.param3 = -1.0f; // Leave unchanged. vehicle_command.param3 = -1.f; // Leave secondary control unchanged
vcmd.param4 = -1.0f; // Leave unchanged. vehicle_command.param4 = -1.f;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
} }
@@ -1556,10 +1554,10 @@ void
Navigator::stop_capturing_images() Navigator::stop_capturing_images()
{ {
if (_is_capturing_images) { if (_is_capturing_images) {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = NAV_CMD_IMAGE_STOP_CAPTURE; vehicle_command.command = NAV_CMD_IMAGE_STOP_CAPTURE;
vcmd.param1 = 0.0f; vehicle_command.param1 = 0.f;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
// _is_capturing_images is reset inside publish_vehicle_cmd. // _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() void Navigator::disable_camera_trigger()
{ {
// Disable camera trigger // Disable camera trigger
vehicle_command_s cmd {}; vehicle_command_s vehicle_command{};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
// Pause trigger // Pause trigger
cmd.param1 = -1.0f; vehicle_command.param1 = -1.f;
cmd.param3 = 1.0f; vehicle_command.param3 = 1.f;
publish_vehicle_cmd(&cmd); publish_vehicle_cmd(vehicle_command);
} }
void Navigator::set_gimbal_neutral() void Navigator::set_gimbal_neutral()
{ {
vehicle_command_s vcmd = {}; vehicle_command_s vehicle_command{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
vcmd.param1 = NAN; vehicle_command.param1 = NAN; // Don't set any angles
vcmd.param2 = NAN; vehicle_command.param2 = NAN;
vcmd.param3 = NAN; vehicle_command.param3 = NAN; // Don't set any angular velocities
vcmd.param4 = NAN; vehicle_command.param4 = NAN;
vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
publish_vehicle_cmd(&vcmd); publish_vehicle_cmd(vehicle_command);
} }
void Navigator::sendWarningDescentStoppedDueToTerrain() void Navigator::sendWarningDescentStoppedDueToTerrain()