Add support for MAV_CMD_SET_CAMERA_SOURCE

PX4 · May 15, 2024 · d97bae2 · d97bae2
1 parent 103c386
commit d97bae2
Show file tree

Hide file tree

Showing 6 changed files with 19 additions and 1 deletion.
diff --git 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/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
@@ -1591,6 +1591,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:
@@ -1688,6 +1689,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
@@ -293,6 +293,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) {
@@ -378,6 +379,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
@@ -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
@@ -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
@@ -1414,6 +1414,18 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 
 		break;
 
+	case NAV_CMD_SET_CAMERA_SOURCE:
+		target_camera_component_id = static_cast<int>(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:
 	case NAV_CMD_VIDEO_STOP_CAPTURE:
 		vcmd->target_component = 100; // MAV_COMP_ID_CAMERA