From da8827883fe248960ab5a3490e9a263ea98e2d72 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 9 Oct 2024 19:10:08 +1100 Subject: [PATCH] Use target camera in image capture start/stop messages (#23115) * Use target camera in image capture start/stop messages * Add support for MAV_CMD_SET_CAMERA_SOURCE * Add target ID for NAV_CMD_SET_CAMERA_MODE * Run make format --- .../camera_trigger/camera_trigger_params.c | 2 +- src/modules/manual_control/ManualControl.cpp | 4 +- src/modules/mavlink/mavlink_mission.cpp | 2 + .../MissionFeasibility/FeasibilityChecker.cpp | 2 + src/modules/navigator/mission_block.cpp | 1 + src/modules/navigator/navigation.h | 1 + src/modules/navigator/navigator_main.cpp | 48 ++++++++++++++++++- 7 files changed, 55 insertions(+), 5 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 3841e20e5642..54e0fd31753b 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -47,7 +47,7 @@ * * @value 1 GPIO * @value 2 Seagull MAP2 (over PWM) -* @value 3 MAVLink (forward via MAV_CMD_IMAGE_START_CAPTURE) +* @value 3 MAVLink (Camera Protocol v1) * @value 4 Generic PWM (IR trigger, servo) * * @reboot_required true diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 53383fbb4a52..2c98f108a0eb 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -453,7 +453,7 @@ void ManualControl::send_camera_mode_command(CameraMode camera_mode) command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE; command.param2 = static_cast(camera_mode); command.target_system = _system_id; - command.target_component = 100; // any camera + command.target_component = 100; // MAV_COMP_ID_CAMERA uORB::Publication command_pub{ORB_ID(vehicle_command)}; command.timestamp = hrt_absolute_time(); @@ -467,7 +467,7 @@ void ManualControl::send_photo_command() command.param3 = 1; // one picture command.param4 = _image_sequence++; command.target_system = _system_id; - command.target_component = 100; // any camera + command.target_component = 100; // MAV_COMP_ID_CAMERA uORB::Publication command_pub{ORB_ID(vehicle_command)}; command.timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index add42698cc98..36d4453344ce 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1607,6 +1607,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_OBLIQUE_SURVEY: case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case MAV_CMD_SET_CAMERA_MODE: + case MAV_CMD_SET_CAMERA_SOURCE: case MAV_CMD_DO_VTOL_TRANSITION: case MAV_CMD_NAV_DELAY: case MAV_CMD_NAV_RETURN_TO_LAUNCH: @@ -1703,6 +1704,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: + case NAV_CMD_SET_CAMERA_SOURCE: case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_VTOL_TRANSITION: diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index c2b3a8119807..dbaf173efa9f 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -286,6 +286,7 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_SOURCE && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { @@ -371,6 +372,7 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_SOURCE && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c2b17bbedbab..89f42e6bb5c8 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -91,6 +91,7 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: + case NAV_CMD_SET_CAMERA_SOURCE: case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_CHANGE_SPEED: diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 87dd05d16675..ecf616ffc454 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -89,6 +89,7 @@ enum NAV_CMD { NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, NAV_CMD_OBLIQUE_SURVEY = 260, NAV_CMD_SET_CAMERA_MODE = 530, + NAV_CMD_SET_CAMERA_SOURCE = 534, NAV_CMD_SET_CAMERA_ZOOM = 531, NAV_CMD_SET_CAMERA_FOCUS = 532, NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cd73e7e7d37c..807ff1284df2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1411,9 +1411,13 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) vcmd->confirmation = false; vcmd->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) { + + case NAV_CMD_IMAGE_START_CAPTURE: if (static_cast(vcmd->param3) == 1) { @@ -1433,12 +1437,52 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) _is_capturing_images = true; } - vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + target_camera_component_id = static_cast(vcmd->param1); // Target id from param 1 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + } + break; case NAV_CMD_IMAGE_STOP_CAPTURE: _is_capturing_images = false; - vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + target_camera_component_id = static_cast(vcmd->param1); // Target id from param 1 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->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 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->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 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + } + break; case NAV_CMD_VIDEO_START_CAPTURE: