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

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

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-18 09:14:51 +01:00 committed by Daniel Agar
parent bf7da6430d
commit 8be64278be
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/navigator/navigator.h
View File

@ -335,6 +335,8 @@ private:
GeofenceBreachAvoidance _gf_breach_avoidance;
hrt_abstime _last_geofence_check = 0;
hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */
bool _geofence_reposition_sent{false}; /**< flag if reposition command has been sent for current geofence breach*/
hrt_abstime _time_loitering_after_gf_breach{0}; /**< timestamp of when loitering after a geofence breach was started */
bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */

19
src/modules/navigator/navigator_main.cpp
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]) {