diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 2914d622a9..e8205b48a6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -981,6 +981,10 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_IMAGE_START_CAPTURE: + case MAV_CMD_IMAGE_STOP_CAPTURE: + case MAV_CMD_VIDEO_START_CAPTURE: + case MAV_CMD_VIDEO_STOP_CAPTURE: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: @@ -1040,6 +1044,10 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_CHANGE_SPEED: case NAV_CMD_DO_SET_SERVO: case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 025beb146b..d7282400c5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -104,6 +104,10 @@ MissionBlock::is_mission_item_reached() return false; case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: @@ -232,7 +236,7 @@ MissionBlock::is_mission_item_reached() // set required yaw from bearing to the next mission item if (_mission_item.force_heading) { struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; - + if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, @@ -352,7 +356,21 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, cmd->command = item->nav_cmd; cmd->target_system = _navigator->get_vstatus()->system_id; - cmd->target_component = _navigator->get_vstatus()->component_id; + + // The camera commands are not processed on the autopilot but will be + // sent to the mavlink links to other components. + switch (item->nav_cmd) { + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: + cmd->target_component = 100; // MAV_COMP_ID_CAMERA + break; + default: + cmd->target_component = _navigator->get_vstatus()->component_id; + break; + } + cmd->source_system = _navigator->get_vstatus()->system_id; cmd->source_component = _navigator->get_vstatus()->component_id; cmd->confirmation = false; @@ -404,6 +422,10 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || item->nav_cmd == NAV_CMD_DO_SET_SERVO || item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || + item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE || + item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE || + item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE || + item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE || item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE || item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || item->nav_cmd == NAV_CMD_DO_SET_ROI || diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 0e9db25a91..1817a14dca 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -248,6 +248,10 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index eb41580dd6..3479e9eaff 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -70,6 +70,10 @@ enum NAV_CMD { NAV_CMD_DO_MOUNT_CONFIGURE=204, NAV_CMD_DO_MOUNT_CONTROL=205, NAV_CMD_DO_SET_CAM_TRIGG_DIST=206, + NAV_CMD_IMAGE_START_CAPTURE=2000, + NAV_CMD_IMAGE_STOP_CAPTURE=2001, + NAV_CMD_VIDEO_START_CAPTURE=2500, + NAV_CMD_VIDEO_STOP_CAPTURE=2501, NAV_CMD_DO_VTOL_TRANSITION=3000, NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */ };