diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 1dffaabd6e..2914d622a9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -983,6 +983,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_SET_ROI: + case NAV_CMD_ROI: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_VTOL_TRANSITION: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; @@ -1040,6 +1042,8 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_SET_ROI: + case NAV_CMD_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: case NAV_CMD_DO_VTOL_TRANSITION: break; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8ed8912c14..025beb146b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -106,6 +106,8 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_SET_ROI: + case NAV_CMD_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: return true; @@ -404,6 +406,8 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE || item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || + item->nav_cmd == NAV_CMD_DO_SET_ROI || + item->nav_cmd == NAV_CMD_ROI || item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 8cbb3ef599..0e9db25a91 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -250,6 +250,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && + missionitem.nav_cmd != NAV_CMD_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 85a4dd8c22..eb41580dd6 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -59,11 +59,13 @@ enum NAV_CMD { NAV_CMD_TAKEOFF = 22, NAV_CMD_LOITER_TO_ALT = 31, NAV_CMD_DO_FOLLOW_REPOSITION = 33, + NAV_CMD_ROI = 80, NAV_CMD_VTOL_TAKEOFF = 84, NAV_CMD_VTOL_LAND = 85, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183, + NAV_CMD_DO_SET_ROI=201, NAV_CMD_DO_DIGICAM_CONTROL=203, NAV_CMD_DO_MOUNT_CONFIGURE=204, NAV_CMD_DO_MOUNT_CONTROL=205,