forked from Archive/PX4-Autopilot
Navigator: Mission: improve survey mission resume
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
24b1ef93b2
commit
18f0311161
|
@ -171,6 +171,8 @@ Mission::on_inactivation()
|
|||
|
||||
/* reset so current mission item gets restarted if mission was paused */
|
||||
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
|
||||
|
||||
_inactivation_index = _current_mission_index;
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -188,15 +190,28 @@ Mission::on_activation()
|
|||
// we already reset the mission items
|
||||
_execution_mode_changed = false;
|
||||
|
||||
set_mission_items();
|
||||
// reset the cache and fill it with the camera and gimbal items up to the previous item
|
||||
if (_current_mission_index > 0) {
|
||||
resetItemCache();
|
||||
updateCachedItemsUpToIndex(_current_mission_index - 1);
|
||||
}
|
||||
|
||||
// unpause triggering if it was paused
|
||||
vehicle_command_s cmd = {};
|
||||
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
|
||||
// unpause trigger
|
||||
cmd.param1 = -1.0f;
|
||||
cmd.param3 = 0.0f;
|
||||
_navigator->publish_vehicle_cmd(&cmd);
|
||||
unsigned resume_index;
|
||||
|
||||
if (_inactivation_index > 0 && cameraWasTriggering()
|
||||
&& getPreviousPositionItemIndex(_mission, _inactivation_index - 1, resume_index)) {
|
||||
// The mission we are resuming had camera triggering enabled. In order to not lose any images
|
||||
// we restart the mission at the previous position item.
|
||||
// We will replay the cached commands once we reach the previous position item and have yaw aligned.
|
||||
set_current_mission_index(resume_index);
|
||||
|
||||
_align_heading_necessary = true;
|
||||
|
||||
} else {
|
||||
set_mission_items();
|
||||
}
|
||||
|
||||
_inactivation_index = -1; // reset
|
||||
|
||||
// reset cruise speed
|
||||
_navigator->reset_cruising_speed();
|
||||
|
@ -235,6 +250,37 @@ Mission::on_active()
|
|||
set_mission_items();
|
||||
}
|
||||
|
||||
// check if heading alignment is necessary, and add it to the current mission item if necessary
|
||||
if (_align_heading_necessary && is_mission_item_reached_or_completed()) {
|
||||
mission_item_s next_position_mission_item = {};
|
||||
|
||||
// add yaw alignment requirement on the current mission item
|
||||
if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_position_mission_item)
|
||||
&& !PX4_ISFINITE(_mission_item.yaw)) {
|
||||
_mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon,
|
||||
next_position_mission_item.lat, next_position_mission_item.lon));
|
||||
_mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode
|
||||
}
|
||||
|
||||
mission_apply_limitation(_mission_item);
|
||||
mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
_align_heading_necessary = false;
|
||||
}
|
||||
|
||||
// replay gimbal and camera commands immediately after resuming mission
|
||||
if (haveCachedGimbalOrCameraItems()) {
|
||||
replayCachedGimbalCameraItems();
|
||||
}
|
||||
|
||||
// replay trigger commands upon raching the resume waypoint if the trigger relay flag is set
|
||||
if (cameraWasTriggering() && is_mission_item_reached_or_completed()) {
|
||||
replayCachedTriggerItems();
|
||||
}
|
||||
|
||||
/* lets check if we reached the current mission item */
|
||||
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) {
|
||||
/* If we just completed a takeoff which was inserted before the right waypoint,
|
||||
|
@ -305,6 +351,11 @@ Mission::set_current_mission_index(uint16_t index)
|
|||
|
||||
_current_mission_index = index;
|
||||
|
||||
// we start from the first item so can reset the cache
|
||||
if (_current_mission_index == 0) {
|
||||
resetItemCache();
|
||||
}
|
||||
|
||||
// a mission index is set manually which has the higher priority than the closest mission item
|
||||
// as it is set by the user
|
||||
_mission_waypoints_changed = false;
|
||||
|
@ -380,6 +431,7 @@ Mission::set_execution_mode(const uint8_t mode)
|
|||
// handle switch from reverse to forward mission
|
||||
if (_current_mission_index < 0) {
|
||||
_current_mission_index = 0;
|
||||
resetItemCache(); // reset cache as we start from the beginning
|
||||
|
||||
} else if (_current_mission_index < _mission.count - 1) {
|
||||
++_current_mission_index;
|
||||
|
@ -578,6 +630,14 @@ Mission::update_mission()
|
|||
_current_mission_index = 0;
|
||||
}
|
||||
|
||||
// we start from the first item so can reset the cache
|
||||
if (_current_mission_index == 0) {
|
||||
resetItemCache();
|
||||
}
|
||||
|
||||
// reset as when we update mission we don't want to proceed at previous index
|
||||
_inactivation_index = -1;
|
||||
|
||||
// find and store landing start marker (if available)
|
||||
find_mission_land_start();
|
||||
|
||||
|
@ -1932,3 +1992,136 @@ void Mission::publish_navigator_mission_item()
|
|||
|
||||
_navigator_mission_item_pub.publish(navigator_mission_item);
|
||||
}
|
||||
|
||||
bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactivation_index,
|
||||
unsigned &prev_pos_index) const
|
||||
{
|
||||
struct mission_item_s missionitem = {};
|
||||
|
||||
for (int index = inactivation_index; index >= 0; index--) {
|
||||
if (!readMissionItemAtIndex(mission, index, missionitem)) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (MissionBlock::item_contains_position(missionitem)) {
|
||||
prev_pos_index = index;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const
|
||||
{
|
||||
while (start_index < mission.count) {
|
||||
if (readMissionItemAtIndex(mission, start_index, mission_item) && MissionBlock::item_contains_position(mission_item)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
start_index++;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Mission::readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem) const
|
||||
{
|
||||
bool success = false;
|
||||
|
||||
if (index >= 0 && index < mission.count) {
|
||||
const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
|
||||
const ssize_t len = sizeof(missionitem);
|
||||
success = (dm_read(dm_current, index, &missionitem, len) == len);
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void Mission::cacheItem(const mission_item_s &mission_item)
|
||||
{
|
||||
switch (mission_item.nav_cmd) {
|
||||
case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
|
||||
_last_gimbal_configure_item = mission_item;
|
||||
break;
|
||||
|
||||
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||
_last_gimbal_control_item = mission_item;
|
||||
break;
|
||||
|
||||
case NAV_CMD_SET_CAMERA_MODE:
|
||||
_last_camera_mode_item = mission_item;
|
||||
break;
|
||||
|
||||
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
||||
case NAV_CMD_DO_TRIGGER_CONTROL:
|
||||
case NAV_CMD_IMAGE_START_CAPTURE:
|
||||
case NAV_CMD_IMAGE_STOP_CAPTURE:
|
||||
_last_camera_trigger_item = mission_item;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Mission::replayCachedGimbalCameraItems()
|
||||
{
|
||||
if (_last_gimbal_configure_item.nav_cmd > 0) {
|
||||
issue_command(_last_gimbal_configure_item);
|
||||
_last_gimbal_configure_item = {}; // delete cached item
|
||||
}
|
||||
|
||||
if (_last_gimbal_control_item.nav_cmd > 0) {
|
||||
issue_command(_last_gimbal_control_item);
|
||||
_last_gimbal_control_item = {}; // delete cached item
|
||||
}
|
||||
|
||||
if (_last_camera_mode_item.nav_cmd > 0) {
|
||||
issue_command(_last_camera_mode_item);
|
||||
_last_camera_mode_item = {}; // delete cached item
|
||||
}
|
||||
}
|
||||
|
||||
void Mission::replayCachedTriggerItems()
|
||||
{
|
||||
if (_last_camera_trigger_item.nav_cmd > 0) {
|
||||
issue_command(_last_camera_trigger_item);
|
||||
_last_camera_trigger_item = {}; // delete cached item
|
||||
}
|
||||
}
|
||||
|
||||
void Mission::resetItemCache()
|
||||
{
|
||||
_last_gimbal_configure_item = {};
|
||||
_last_gimbal_control_item = {};
|
||||
_last_camera_mode_item = {};
|
||||
_last_camera_trigger_item = {};
|
||||
}
|
||||
|
||||
bool Mission::haveCachedGimbalOrCameraItems()
|
||||
{
|
||||
return _last_gimbal_configure_item.nav_cmd > 0 ||
|
||||
_last_gimbal_control_item.nav_cmd > 0 ||
|
||||
_last_camera_mode_item.nav_cmd > 0;
|
||||
}
|
||||
|
||||
bool Mission::cameraWasTriggering()
|
||||
{
|
||||
return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL
|
||||
&& (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) ||
|
||||
(_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) ||
|
||||
(_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST
|
||||
&& _last_camera_trigger_item.params[0] > FLT_EPSILON);
|
||||
}
|
||||
|
||||
void Mission::updateCachedItemsUpToIndex(const int end_index)
|
||||
{
|
||||
for (int i = 0; i <= end_index; i++) {
|
||||
mission_item_s mission_item = {};
|
||||
|
||||
if (readMissionItemAtIndex(_mission, i, mission_item)) {
|
||||
cacheItem(mission_item);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -240,6 +240,80 @@ private:
|
|||
|
||||
void publish_navigator_mission_item();
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the index associated with the last item that contains a position
|
||||
* @param mission The mission to search
|
||||
* @param start_index The index to start searching from
|
||||
* @param prev_pos_index The index of the previous position item containing a position
|
||||
* @return true if a previous position item was found
|
||||
*/
|
||||
bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index) const;
|
||||
|
||||
/**
|
||||
* @brief Get the next item after start_index that contains a position
|
||||
*
|
||||
* @param mission The mission to search
|
||||
* @param start_index The index to start searching from
|
||||
* @param mission_item The mission item to populate
|
||||
* @return true if successful
|
||||
*/
|
||||
bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) const;
|
||||
|
||||
/**
|
||||
* @brief Read the mission item at the given index
|
||||
*
|
||||
* @param mission The mission to read from
|
||||
* @param index The index to read
|
||||
* @param missionitem The mission item to populate
|
||||
* @return true if successful
|
||||
*/
|
||||
bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem) const;
|
||||
|
||||
/**
|
||||
* @brief Cache the mission items containing gimbal, camera mode and trigger commands
|
||||
*
|
||||
* @param mission_item The mission item to cache if applicable
|
||||
*/
|
||||
void cacheItem(const mission_item_s &mission_item);
|
||||
|
||||
/**
|
||||
* @brief Update the cached items up to the given index
|
||||
*
|
||||
* @param end_index The index to update up to
|
||||
*/
|
||||
void updateCachedItemsUpToIndex(int end_index);
|
||||
|
||||
/**
|
||||
* @brief Replay the cached gimbal and camera mode items
|
||||
*/
|
||||
void replayCachedGimbalCameraItems();
|
||||
|
||||
/**
|
||||
* @brief Replay the cached trigger items
|
||||
*
|
||||
*/
|
||||
void replayCachedTriggerItems();
|
||||
|
||||
/**
|
||||
* @brief Reset the item cache
|
||||
*/
|
||||
void resetItemCache();
|
||||
|
||||
/**
|
||||
* @brief Check if there are cached gimbal or camera mode items to be replayed
|
||||
*
|
||||
* @return true if there are cached items
|
||||
*/
|
||||
bool haveCachedGimbalOrCameraItems();
|
||||
|
||||
/**
|
||||
* @brief Check if the camera was triggering
|
||||
*
|
||||
* @return true if there was a camera trigger command in the cached items that didn't disable triggering
|
||||
*/
|
||||
bool cameraWasTriggering();
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
|
||||
(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
|
||||
|
@ -291,4 +365,12 @@ private:
|
|||
|
||||
uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
|
||||
bool _execution_mode_changed{false};
|
||||
|
||||
int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images.
|
||||
bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment.
|
||||
|
||||
mission_item_s _last_gimbal_configure_item {};
|
||||
mission_item_s _last_gimbal_control_item {};
|
||||
mission_item_s _last_camera_mode_item {};
|
||||
mission_item_s _last_camera_trigger_item {};
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue