fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/navigator/land.cpp
View File

@ -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);
}
}

18
src/modules/navigator/mission_base.cpp
View File

@ -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()

36
src/modules/navigator/mission_block.cpp
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
// 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<double>(item.params[4]);
vcmd.param6 = static_cast<double>(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<double>(item.params[4]);
vehicle_command.param6 = static_cast<double>(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 */

8
src/modules/navigator/navigator.h
View File

@ -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
/**

158
src/modules/navigator/navigator_main.cpp
View File

@ -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<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
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<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) {
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<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) {
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<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) {
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<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) {
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()