Browse Source

allow DO_CONTROL_VIDEO in missions

sbg
David Jablonski 5 years ago committed by Julian Oes
parent
commit
536cd6cb1a
  1. 1
      msg/actuator_controls.msg
  2. 2
      src/modules/mavlink/mavlink_mission.cpp
  3. 1
      src/modules/mavlink/mavlink_receiver.cpp
  4. 1
      src/modules/navigator/mission_block.cpp
  5. 2
      src/modules/navigator/mission_feasibility_checker.cpp
  6. 1
      src/modules/navigator/navigation.h

1
msg/actuator_controls.msg

@ -9,7 +9,6 @@ uint8 INDEX_FLAPS = 4
uint8 INDEX_SPOILERS = 5 uint8 INDEX_SPOILERS = 5
uint8 INDEX_AIRBRAKES = 6 uint8 INDEX_AIRBRAKES = 6
uint8 INDEX_LANDING_GEAR = 7 uint8 INDEX_LANDING_GEAR = 7
uint8 INDEX_GIMBAL_SHUTTER = 3 uint8 INDEX_GIMBAL_SHUTTER = 3
uint8 INDEX_CAMERA_ZOOM = 4 uint8 INDEX_CAMERA_ZOOM = 4

2
src/modules/mavlink/mavlink_mission.cpp

@ -1473,6 +1473,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case MAV_CMD_SET_CAMERA_MODE: case MAV_CMD_SET_CAMERA_MODE:
@ -1546,6 +1547,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_CONTROL_VIDEO:
case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI:

1
src/modules/mavlink/mavlink_receiver.cpp

@ -533,6 +533,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE: case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE:
actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = cmd_mavlink.param2 / 50.0f - 1.0f; actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = cmd_mavlink.param2 / 50.0f - 1.0f;
break; break;
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_STEP: case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_STEP:
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS: case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS:
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH: case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH:

1
src/modules/navigator/mission_block.cpp

@ -93,6 +93,7 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_CONTROL_VIDEO:
case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI:

2
src/modules/navigator/mission_feasibility_checker.cpp

@ -259,6 +259,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
@ -384,6 +385,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&

1
src/modules/navigator/navigation.h

@ -76,6 +76,7 @@ enum NAV_CMD {
NAV_CMD_DO_SET_ROI_LOCATION = 195, NAV_CMD_DO_SET_ROI_LOCATION = 195,
NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196, NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196,
NAV_CMD_DO_SET_ROI_NONE = 197, NAV_CMD_DO_SET_ROI_NONE = 197,
NAV_CMD_DO_CONTROL_VIDEO = 200,
NAV_CMD_DO_SET_ROI = 201, NAV_CMD_DO_SET_ROI = 201,
NAV_CMD_DO_DIGICAM_CONTROL = 203, NAV_CMD_DO_DIGICAM_CONTROL = 203,
NAV_CMD_DO_MOUNT_CONFIGURE = 204, NAV_CMD_DO_MOUNT_CONFIGURE = 204,

Loading…
Cancel
Save