Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 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);
src/modules/commander/Commander.hpp
+1 -1
View File
@@ -401,7 +401,6 @@ private:
// Subscriptions
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
@@ -415,6 +414,7 @@ private:
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _wind_sub{ORB_ID(wind)};