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

fix navigator: prevent race condition when receiving multiple commands at once

Browse Source 
When handling multiple commands, it could happen that the first command
updates _reposition_triplet. Normally this would then get handled after
getting the mode change from commander through vehicle_status.
But if the next command is handled before an update from commander, it
could overwrite the triplet.
This patch ensures that navigator waits for an update from commander (and
therefore process the _reposition_triplet) before handling the next
command.

This happened specifically when pressing 'Pause' from QGC during a mission.
QGC sends VEHICLE_CMD_DO_REPOSITION twice, first for pausing, then changing
the altitude.
The result was that the vehicle would not stop at the current location but
continue to the next mission waypoint and stop there.
This commit is contained in:
Beat Küng
2024-01-22 12:45:54 -05:00
committed by Daniel Agar
parent bf7da6430d
commit 8be64278be
2 changed files with 20 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+18 -1
View File
@@ -239,7 +239,8 @@ void Navigator::run()
// Handle Vehicle commands
int vehicle_command_updates = 0;
while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) {
while (_wait_for_vehicle_status_timestamp == 0 && _vehicle_command_sub.updated()
&& (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) {
vehicle_command_updates++;
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
@@ -261,6 +262,9 @@ void Navigator::run()
// only update the reposition setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter,
// which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too)
// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
_wait_for_vehicle_status_timestamp = hrt_absolute_time();
vehicle_global_position_s position_setpoint{};
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
@@ -420,6 +424,9 @@ void Navigator::run()
position_setpoint.lon = get_global_position()->lon;
position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
_wait_for_vehicle_status_timestamp = hrt_absolute_time();
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@@ -493,6 +500,9 @@ void Navigator::run()
position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon;
position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
_wait_for_vehicle_status_timestamp = hrt_absolute_time();
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
@@ -538,6 +548,9 @@ void Navigator::run()
position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon;
position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
// Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten
_wait_for_vehicle_status_timestamp = hrt_absolute_time();
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
@@ -869,6 +882,10 @@ void Navigator::run()
_navigation_mode = navigation_mode_new;
if (_wait_for_vehicle_status_timestamp != 0 && _vstatus.timestamp > _wait_for_vehicle_status_timestamp) {
_wait_for_vehicle_status_timestamp = 0;
}
/* iterate through navigation modes and set active/inactive for each */
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
if (_navigation_mode_array[i]) {