update to match new feature as a new mavlink command Oblique Survey 260

msg/vehicle_command.msg

@@ -65,6 +65,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242	# Set sensor offsets. This
 uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260			# Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|
 uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes

src/drivers/camera_trigger/camera_trigger.cpp

@@ -183,7 +183,6 @@ private:
 	float			_pseudo_oblique_roll_angle;
 	float			_pseudo_oblique_angle_interval;
 	float			_pseudo_oblique_pitch_angle;
-	float			_move_distance;
 	bool			_updated_roll_angle;
 	uint32_t		_target_system;
 	uint32_t		_target_component;
@@ -276,7 +275,6 @@ CameraTrigger::CameraTrigger() :
 	_pseudo_oblique_roll_angle(0.0f),
 	_pseudo_oblique_angle_interval(0.0f),
 	_pseudo_oblique_pitch_angle(-90),
-	_move_distance(0.0f),
 	_updated_roll_angle(false),
 	_target_system(0),
 	_target_component(0),
@@ -410,15 +408,15 @@ CameraTrigger::update_distance()
 			}
 		}
 
-		float current_distance = matrix::Vector2f(_last_shoot_position - current_position).length();
+		hrt_abstime now = hrt_absolute_time();
 
-		if (_pseudo_oblique_num_poses > 0 && current_distance > _move_distance && !_updated_roll_angle) {
+		if (!_updated_roll_angle && _pseudo_oblique_num_poses > 0 && (now - _last_trigger_timestamp > _min_interval * 1000)) {
 			adjust_roll();
 			_updated_roll_angle = true;
 		}
 
 		// Check that distance threshold is exceeded
-		if (current_distance >= _distance) {
+		if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
 			shoot_once();
 			_updated_roll_angle = false;
 			_last_shoot_position = current_position;
@@ -650,44 +648,79 @@ CameraTrigger::Run()
 				_one_shot = true;
 			}
 
-			// Camera Auto Mount Pseudo Oblique Solution (CAMPOS)
-			if (cmd.param4 >= 2.0f) {
-				_pseudo_oblique_num_poses = commandParamToInt(cmd.param4);
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
-				if (cmd.param5 > 0.0) {
-					_pseudo_oblique_roll_angle = cmd.param5;
+		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
 
-				} else {
-					_pseudo_oblique_roll_angle = 30.0f;
-				}
+			need_ack = true;
 
-				_pseudo_oblique_pitch_angle = cmd.param6;
-				_pseudo_oblique_angle_interval = _pseudo_oblique_roll_angle * 2 / (_pseudo_oblique_num_poses - 1);
-				_pseudo_oblique_pose_counter = 0;
-				_move_distance = _distance / 2;
-				_updated_roll_angle = false;
+			if (cmd.param1 > 0.0f) {
+				_interval = cmd.param1;
+				param_set_no_notification(_p_interval, &(_interval));
+			}
 
-			} else {
-				_pseudo_oblique_num_poses = 0;
+			// We can only control the shutter integration time of the camera in GPIO mode
+			if (cmd.param2 > 0.0f) {
+				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
+					_activation_time = cmd.param2;
+					param_set_no_notification(_p_activation_time, &_activation_time);
+				}
 			}
 
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
+		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
+
+			// Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
 
 			need_ack = true;
 
 			if (cmd.param1 > 0.0f) {
-				_interval = cmd.param1;
-				param_set_no_notification(_p_interval, &(_interval));
+				_distance = cmd.param1;
+				param_set_no_notification(_p_distance, &_distance);
+
+				_trigger_enabled = true;
+				_trigger_paused = false;
+				_valid_position = false;
+
+			} else if (commandParamToInt(cmd.param1) == 0) {
+				_trigger_paused = true;
+
+			} else if (commandParamToInt(cmd.param1) == -1) {
+				_trigger_enabled = false;
 			}
 
-			// We can only control the shutter integration time of the camera in GPIO mode
+			// We can only control the shutter integration time of the camera in GPIO mode (for now)
 			if (cmd.param2 > 0.0f) {
 				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
 					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &_activation_time);
+					param_set_no_notification(_p_activation_time, &(_activation_time));
+				}
+			}
+
+			// Set Param for minimum camera trigger interval
+			if (cmd.param3 > 0.0f) {
+				_min_interval = cmd.param3;
+				param_set_no_notification(_p_min_interval, &(_min_interval));
+			}
+
+			if (cmd.param4 >= 2.0f) {
+				_pseudo_oblique_num_poses = commandParamToInt(cmd.param4);
+
+				if (cmd.param5 > 0.0) {
+					_pseudo_oblique_roll_angle = cmd.param5;
+
+				} else {
+					_pseudo_oblique_roll_angle = 30.0f;
 				}
+
+				_pseudo_oblique_pitch_angle = cmd.param6;
+				_pseudo_oblique_angle_interval = _pseudo_oblique_roll_angle * 2 / (_pseudo_oblique_num_poses - 1);
+				_pseudo_oblique_pose_counter = 0;
+				_updated_roll_angle = false;
+
+			} else {
+				_pseudo_oblique_num_poses = 0;
 			}
 
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

src/modules/commander/Commander.cpp

@@ -1227,6 +1227,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
+	case vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE:
 	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM:

src/modules/mavlink/mavlink_mission.cpp

@@ -1466,6 +1466,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_VIDEO_STOP_CAPTURE:
 		case MAV_CMD_DO_CONTROL_VIDEO:
 		case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+		case MAV_CMD_OBLIQUE_SURVEY:
 		case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 		case MAV_CMD_SET_CAMERA_MODE:
 		case MAV_CMD_SET_CAMERA_ZOOM:
@@ -1543,6 +1544,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		case NAV_CMD_DO_MOUNT_CONTROL:
 		case NAV_CMD_DO_SET_ROI:
 		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
+		case NAV_CMD_OBLIQUE_SURVEY:
 		case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 		case NAV_CMD_SET_CAMERA_MODE:
 		case NAV_CMD_SET_CAMERA_ZOOM:

src/modules/navigator/mission_block.cpp

@@ -101,6 +101,7 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 	case NAV_CMD_DO_SET_ROI_NONE:
 	case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
+	case NAV_CMD_OBLIQUE_SURVEY:
 	case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 	case NAV_CMD_SET_CAMERA_MODE:
 	case NAV_CMD_SET_CAMERA_ZOOM:

src/modules/navigator/mission_feasibility_checker.cpp

@@ -267,6 +267,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
+		    missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
@@ -393,6 +394,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
+					  missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY &&
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&

src/modules/navigator/navigation.h

@@ -83,6 +83,7 @@ enum NAV_CMD {
 	NAV_CMD_DO_MOUNT_CONTROL = 205,
 	NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
+	NAV_CMD_OBLIQUE_SURVEY = 260,
 	NAV_CMD_SET_CAMERA_MODE = 530,
 	NAV_CMD_SET_CAMERA_ZOOM = 531,
 	NAV_CMD_IMAGE_START_CAPTURE = 2000,