fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: only process one vehicle_command/action_request per cycle

Browse Source 
- things like arming requests can be dependent on current nav state
that might requested by a previous command, but the state machine
transition will only happen after command processing
This commit is contained in:
Daniel Agar
2022-03-23 16:26:06 -04:00
parent e528eb5dc4
commit 879622547c
2 changed files with 16 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+15 -7
View File
@@ -2710,14 +2710,14 @@ Commander::run()
}
/* handle commands last, as the system needs to be updated to handle them */
while (_cmd_sub.updated()) {
if (_vehicle_command_sub.updated()) {
/* got command */
const unsigned last_generation = _cmd_sub.get_last_generation();
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
vehicle_command_s cmd;
if (_cmd_sub.copy(&cmd)) {
if (_cmd_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _cmd_sub.get_last_generation());
if (_vehicle_command_sub.copy(&cmd)) {
if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
}
if (handle_command(cmd)) {
@@ -2726,10 +2726,15 @@ Commander::run()
}
}
while (_action_request_sub.updated()) {
if (_action_request_sub.updated()) {
const unsigned last_generation = _action_request_sub.get_last_generation();
action_request_s action_request;
if (_action_request_sub.copy(&action_request)) {
if (_action_request_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("action_request lost, generation %u -> %u", last_generation, _action_request_sub.get_last_generation());
}
executeActionRequest(action_request);
}
}
@@ -3080,7 +3085,10 @@ Commander::run()
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
px4_usleep(COMMANDER_MONITORING_INTERVAL);
// sleep if there are no vehicle_commands or action_requests to process
if (!_vehicle_command_sub.updated() && !_action_request_sub.updated()) {
px4_usleep(COMMANDER_MONITORING_INTERVAL);
}
}
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);