fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: Resume mission with last flight speed (#21714)

Browse Source 
* Navigator: DO_CHANGE_SPEED: only store sinlge cruising_speed_current_mode

This stored cruising speed setpoint is reset on mode change and
after a VTOL transition.

* Navigator Mission: replay DO_CHANGE_SPEED items when resuming mission

* Navigator: remove cruising_speed_sp_update()

Speed changes in a mission are handled directly in the position controllers,
and no longer in Navigator.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-08-08 13:22:05 +02:00
committed by GitHub
parent 02a6640000
commit 56dd1dc930
4 changed files with 36 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+21 -25
View File
@@ -212,7 +212,8 @@ Mission::on_activation()
// we already reset the mission items
_execution_mode_changed = false;
// reset the cache and fill it with the camera and gimbal items up to the previous item
// reset the cache and fill it with the items up to the previous item. The cache contains
// commands that are valid for the whole mission, not just a sinlge waypoint.
if (_current_mission_index > 0) {
resetItemCache();
updateCachedItemsUpToIndex(_current_mission_index - 1);
@@ -303,6 +304,8 @@ Mission::on_active()
replayCachedTriggerItems();
}
replayCachedSpeedChangeItems();
/* 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,
@@ -324,11 +327,6 @@ Mission::on_active()
}
}
/* check if a cruise speed change has been commanded */
if (_mission_type != MISSION_TYPE_NONE) {
cruising_speed_sp_update();
}
/* see if we need to update the current yaw heading */
if (!_param_mis_mnt_yaw_ctl.get()
&& (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
@@ -1520,25 +1518,6 @@ Mission::heading_sp_update()
}
}
void
Mission::cruising_speed_sp_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
const float cruising_speed = _navigator->get_cruising_speed();
/* Don't change setpoint if the current waypoint is not valid */
if (!pos_sp_triplet->current.valid ||
fabsf(pos_sp_triplet->current.cruising_speed - cruising_speed) < FLT_EPSILON) {
return;
}
pos_sp_triplet->current.cruising_speed = cruising_speed;
publish_navigator_mission_item();
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::do_abort_landing()
{
@@ -2106,6 +2085,15 @@ void Mission::cacheItem(const mission_item_s &mission_item)
_last_camera_trigger_item = mission_item;
break;
case NAV_CMD_DO_CHANGE_SPEED:
_last_speed_change_item = mission_item;
break;
case NAV_CMD_DO_VTOL_TRANSITION:
// delete speed changes after a VTOL transition
_last_speed_change_item = {};
break;
default:
break;
}
@@ -2137,6 +2125,14 @@ void Mission::replayCachedTriggerItems()
}
}
void Mission::replayCachedSpeedChangeItems()
{
if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) {
issue_command(_last_speed_change_item);
_last_speed_change_item = {}; // delete cached item
}
}
void Mission::resetItemCache()
{
_last_gimbal_configure_item = {};