dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Igor Campos 2020-11-11 23:28:11 -04:00 committed by Lorenz Meier
parent 390ed3765f
commit c316af6ec7
7 changed files with 67 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_command.msg
View File

@ -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

85
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -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,27 +648,6 @@ CameraTrigger::Run()
_one_shot = true;
}
// Camera Auto Mount Pseudo Oblique Solution (CAMPOS)
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;
_move_distance = _distance / 2;
_updated_roll_angle = false;
} else {
_pseudo_oblique_num_poses = 0;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
@ -692,6 +669,62 @@ CameraTrigger::Run()
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} 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) {
_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 (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));
}
}
// 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;
} else {
goto unknown_cmd;
}

1
src/modules/commander/Commander.cpp
View File

@ -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:

2
src/modules/mavlink/mavlink_mission.cpp
View File

@ -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:

1
src/modules/navigator/mission_block.cpp
View File

@ -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:

2
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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 &&

1
src/modules/navigator/navigation.h
View File

@ -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,