From b0e1cc72f7308bce1745d447f2f133e1c02c9bdf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 26 Oct 2022 16:15:05 +0200 Subject: [PATCH] fix orbit for mc: handle VEHICLE_CMD_DO_ORBIT command and avoid race condition Fixes a regression from 8bae4e5c0e29f70d5d1d2427ffdef97092be939c, where the orbit flight task wasn't an extra task (flight_tasks_to_add) anymore and therefore the command handling wasn't generated. There was a race condition that could cause several outcomes. The most severe was that flight_mode_manager gets the command, switches to orbit and then in the next iteration switches back because commander did not change nav_state yet. When commander then switches, flight_mode_manager would still be in the old mode. This is prevented by storing the command (allowing it to arrive before or after mode switch), and then apply it after the switch happens. --- .../flight_mode_manager/CMakeLists.txt | 7 -- .../flight_mode_manager/FlightModeManager.cpp | 66 ++++++++++++------- .../flight_mode_manager/FlightModeManager.hpp | 6 +- .../Templates/FlightTasks_generated.cpp.em | 20 ------ .../generate_flight_tasks.py | 2 - .../tasks/FlightTask/FlightTask.hpp | 5 +- .../tasks/Orbit/FlightTaskOrbit.cpp | 16 +++-- .../tasks/Orbit/FlightTaskOrbit.hpp | 2 +- 8 files changed, 60 insertions(+), 64 deletions(-) diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index f8b495e29f..bee40bbc66 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -71,13 +71,6 @@ set(python_args -f ${files_to_generate} ) -# add the additional tasks for the python script (if there are any) -if(flight_tasks_to_add) - list(APPEND python_args - -s ${flight_tasks_to_add} - ) -endif() - # generate the files using the python script and template add_custom_command( OUTPUT diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 8c70e09f12..5dbe29b726 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -114,6 +114,8 @@ void FlightModeManager::Run() handleCommand(); } + tryApplyCommandIfAny(); + if (isAnyTaskActive()) { generateTrajectorySetpoint(dt, vehicle_local_position); } @@ -231,6 +233,19 @@ void FlightModeManager::start_flight_task() if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) { should_disable_task = false; + + if (!_command_failed) { + FlightTaskError error = FlightTaskError::InvalidTask; + +#if !defined(CONSTRAINED_FLASH) + error = switchTask(FlightTaskIndex::Orbit); +#endif // !CONSTRAINED_FLASH + + if (error != FlightTaskError::NoError) { + task_failure = true; + } + + } } // check task failure @@ -265,39 +280,39 @@ void FlightModeManager::start_flight_task() } +void FlightModeManager::tryApplyCommandIfAny() +{ + if (isAnyTaskActive() && _current_command.command != 0 && hrt_absolute_time() < _current_command.timestamp + 200_ms) { + bool success = false; + + if (_current_task.task->applyCommandParameters(_current_command, success)) { + _current_command.command = 0; + + if (!success) { + switchTask(FlightTaskIndex::Failsafe); + _command_failed = true; + } + } + } +} + void FlightModeManager::handleCommand() { // get command vehicle_command_s command; while (_vehicle_command_sub.update(&command)) { - // check what command it is - FlightTaskIndex desired_task = switchVehicleCommand(command.command); - // ignore all unknown commands - if (desired_task != FlightTaskIndex::None - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - // switch to the commanded task - bool switch_succeeded = (switchTask(desired_task) == FlightTaskError::NoError); - uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + switch (command.command) { + case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: + // The command might trigger a mode switch, and the mode switch can happen before or + // after we receive the command here, so we store it for later. + memcpy(&_current_command, &command, sizeof(vehicle_command_s)); + _command_failed = false; + break; + } - // if we are in/switched to the desired task - if (switch_succeeded) { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - - // if the task is running apply parameters to it and see if it rejects - if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; - - // if we just switched and parameters are not accepted, go to failsafe - if (switch_succeeded) { - switchTask(FlightTaskIndex::Failsafe); - } - } - } - - - } else if (_current_task.task) { + if (_current_task.task) { // check for other commands not related to task switching if ((command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) && (static_cast(command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_GROUNDSPEED) @@ -411,6 +426,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) } _current_task.task->setResetCounters(last_reset_counters); + _command_failed = false; return FlightTaskError::NoError; } diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 7f7aea1171..db5fb0bf56 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -111,9 +111,10 @@ private: */ bool isAnyTaskActive() const { return _current_task.task; } + void tryApplyCommandIfAny(); + // generated int _initTask(FlightTaskIndex task_index); - FlightTaskIndex switchVehicleCommand(const int command); /** * Union with all existing tasks: we use it to make sure that only the memory of the largest existing @@ -135,6 +136,9 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration + vehicle_command_s _current_command{}; + bool _command_failed{false}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; diff --git a/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em index 16cedf4056..a1f38d14d1 100644 --- a/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em +++ b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em @@ -75,23 +75,3 @@ int FlightModeManager::_initTask(FlightTaskIndex task_index) _current_task.index = task_index; return 0; } - -FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command) -{ - switch (command) { -@# loop through all additional tasks -@[if tasks_add]@ -@[for task in tasks_add]@ -@{ -upperCase = lambda s: s[:].upper() if s else '' -}@ - case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) : - return FlightTaskIndex::@(task); - break; - -@[end for]@ -@[end if]@ - // ignore all unknown commands - default : return FlightTaskIndex::None; - } -} diff --git a/src/modules/flight_mode_manager/generate_flight_tasks.py b/src/modules/flight_mode_manager/generate_flight_tasks.py index 9bbb192f0e..99f6266b3a 100644 --- a/src/modules/flight_mode_manager/generate_flight_tasks.py +++ b/src/modules/flight_mode_manager/generate_flight_tasks.py @@ -6,7 +6,6 @@ import argparse parser = argparse.ArgumentParser() parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated") -parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)") parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory") parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory") parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate") @@ -20,7 +19,6 @@ for gen_file in args.gen_files: # need to specify the em_globals inside the loop -> em.Error: interpreter globals collision em_globals = { "tasks": args.tasks_all, - "tasks_add": args.tasks_add, } interpreter = em.Interpreter(output=output_file, globals=em_globals) interpreter.file(open(args.directory_in + "/" + gen_file + ".em")) diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index c76a83206a..1df5351fb4 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -91,9 +91,10 @@ public: /** * To be called to adopt parameters from an arrived vehicle command * @param command received command message containing the parameters - * @return true if accepted, false if declined + * @param success set to true if it was successfully applied, false on error + * @return true if handled */ - virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; } + virtual bool applyCommandParameters(const vehicle_command_s &command, bool &success) { return false; } /** * Call before activate() or update() diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index b1a0e70ffb..ea68b452fc 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -49,9 +49,13 @@ FlightTaskOrbit::FlightTaskOrbit() _sticks_data_required = false; } -bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) +bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, bool &success) { - bool ret = true; + if (command.command != vehicle_command_s::VEHICLE_CMD_DO_ORBIT) { + return false; + } + + success = true; // save previous velocity and rotation direction bool new_is_clockwise = _orbit_velocity > 0; float new_radius = _orbit_radius; @@ -81,7 +85,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) } else { mavlink_log_critical(&_mavlink_log_pub, "Orbit radius limit exceeded\t"); events::send(events::ID("orbit_radius_exceeded"), events::Log::Alert, "Orbit radius limit exceeded"); - ret = false; + success = false; } // commanded heading behaviour @@ -98,7 +102,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) _center.xy() = _geo_projection.project(command.param5, command.param6); } else { - ret = false; + success = false; } } @@ -108,7 +112,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) _center(2) = _global_local_alt0 - command.param7; } else { - ret = false; + success = false; } } @@ -118,7 +122,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) _position_smoothing.reset(_acceleration_setpoint, _velocity_setpoint, _position); } - return ret; + return true; } bool FlightTaskOrbit::sendTelemetry() diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 900c8e469a..187a47de41 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -56,7 +56,7 @@ public: FlightTaskOrbit(); virtual ~FlightTaskOrbit() = default; - bool applyCommandParameters(const vehicle_command_s &command) override; + bool applyCommandParameters(const vehicle_command_s &command, bool &success) override; bool activate(const trajectory_setpoint_s &last_setpoint) override; bool update() override;