diff --git a/docs/en/camera/camera_architecture.md b/docs/en/camera/camera_architecture.md index 2919e2bb05f4..b7de079bc247 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_cmd` to publish it (`_navigator->publish_vehicle_cmd(&vcmd);`) - - [`void Navigator::publish_vehicle_cmd(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#L1395) + - 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 525fd96d5f0a..0a3301c9fb6b 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_cmd(&vcmd); + _navigator.publish_vehicle_command(vehicle_command); Publishing command: -void Navigator::publish_vehicle_cmd(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#L1395 + 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 b14dc2191368..6620af5b06e9 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_cmd` to publish it (`_navigator->publish_vehicle_cmd(&vcmd);`) - - [`void Navigator::publish_vehicle_cmd(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#L1395) + - 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 f288f885ccae..0475cf99cc2c 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_cmd(&vcmd); + _navigator->publish_vehicle_command(vehicle_command); Publishing command: -void Navigator::publish_vehicle_cmd(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#L1395 + 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 7dc5adac6813..acd1d2a7953b 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_cmd` для публікації її (`_navigator->publish_vehicle_cmd(&vcmd);`) - - [`void Navigator::publish_vehicle_cmd(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#L1395) + - Для деяких команд камери це встановлює ідентифікатор компонента на ідентифікатор компонента камери (`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 a127df0ac42b..7ca8eeaad92d 100644 --- a/docs/uk/camera/mavlink_v2_camera.md +++ b/docs/uk/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_cmd(&vcmd); + _navigator->publish_vehicle_command(&vehicle_command); Publishing command: -void Navigator::publish_vehicle_cmd(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#L1395 + 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 e62876772361..0adab6b1ca6a 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_cmd` to publish it (`_navigator->publish_vehicle_cmd(&vcmd);`) - - [`void Navigator::publish_vehicle_cmd(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#L1395) + - 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 89a9f73f19ab..3282e25ecb9f 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_cmd(&vcmd); + _navigator->publish_vehicle_command(vehicle_command); Publishing command: -void Navigator::publish_vehicle_cmd(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#L1395 + 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/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 5cb75222a043..f23ec7dc02eb 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -235,7 +235,7 @@ void FlightTaskAuto::_prepareLandSetpoints() bool range_dist_available = PX4_ISFINITE(_dist_to_bottom); if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) { - vertical_speed = _param_mpc_land_crawl_speed.get(); + vertical_speed = _param_mpc_land_crwl.get(); } if (_type_previous != WaypointType::land) { diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 855958025166..c177a74d5f96 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -165,20 +165,16 @@ class FlightTaskAuto : public FlightTask (ParamFloat) _param_mpc_xy_traj_p, (ParamFloat) _param_mpc_xy_err_max, (ParamFloat) _param_mpc_land_speed, - (ParamFloat) _param_mpc_land_crawl_speed, + (ParamFloat) _param_mpc_land_crwl, (ParamInt) _param_mpc_land_rc_help, (ParamFloat) _param_mpc_land_radius, - (ParamFloat) - _param_mpc_land_alt1, // altitude at which we start ramping down speed - (ParamFloat) - _param_mpc_land_alt2, // altitude at which we descend at land speed - (ParamFloat) - _param_mpc_land_alt3, // altitude where we switch to crawl speed, if LIDAR available + (ParamFloat) _param_mpc_land_alt1, + (ParamFloat) _param_mpc_land_alt2, + (ParamFloat) _param_mpc_land_alt3, (ParamFloat) _param_mpc_z_v_auto_up, (ParamFloat) _param_mpc_z_v_auto_dn, (ParamFloat) _param_mpc_tko_speed, - (ParamFloat) - _param_mpc_tko_ramp_t // time constant for smooth takeoff ramp + (ParamFloat) _param_mpc_tko_ramp_t ); private: diff --git a/src/modules/mc_pos_control/multicopter_takeoff_land_params.c b/src/modules/mc_pos_control/multicopter_takeoff_land_params.c index 328a8531ad93..77444cc65b56 100644 --- a/src/modules/mc_pos_control/multicopter_takeoff_land_params.c +++ b/src/modules/mc_pos_control/multicopter_takeoff_land_params.c @@ -89,9 +89,8 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.f); /** * Altitude for 3. step of slow landing * - * Below this altitude descending velocity gets - * limited to "MPC_LAND_CRWL", if LIDAR available. - * No effect if LIDAR not available + * If a valid distance sensor measurement to the ground is available, + * limit descending velocity to "MPC_LAND_CRWL" below this altitude. * * @unit m * @min 0 diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 2c351ffa6e9f..55116c78e3df 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; // Default speed + vehicle_command.param2 = 1.f; // Modes should switch, not setting this is unsupported + 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_command(vehicle_command); } } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index c24a791f8ddc..daac61e933bc 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; // Default speed + vehicle_command.param2 = 1.f; // Modes should switch, not setting this is unsupported + vehicle_command.param5 = _mission_item.lat; + vehicle_command.param6 = _mission_item.lon; + vehicle_command.param7 = alt_sp; + _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 1b05628a6c69..3f2c621baa52 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_command(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; // normal unforced transition + _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 5e30d22ccc95..30b9cd769a69 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -132,12 +132,12 @@ class Navigator : public ModuleBase, public ModuleParams /** * @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 vehicle_command Vehicle command to publish */ - void publish_vehicle_cmd(vehicle_command_s *vcmd); + 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 34b4d2c00e25..dc53c6928dad 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_command(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_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"); @@ -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_command(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_command(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_command(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_command(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) { - // 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; + 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 } 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_command(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_command(vehicle_command); } @@ -1556,12 +1554,12 @@ 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_command(vehicle_command); - // _is_capturing_images is reset inside publish_vehicle_cmd. + // _is_capturing_images is reset inside publish_vehicle_command. } } @@ -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_command(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_command(vehicle_command); } void Navigator::sendWarningDescentStoppedDueToTerrain()