From 57c7b5e843f6a345ff9961646808cae56742bc5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 3 Sep 2022 10:32:11 +0200 Subject: [PATCH] flight_mode_manager: reduce error verbosity & remove mode switching While disarmed, commander allows to be in any mode now. --- .../flight_mode_manager/FlightModeManager.cpp | 153 +++--------------- .../flight_mode_manager/FlightModeManager.hpp | 10 +- 2 files changed, 24 insertions(+), 139 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 101da19251..7fff71f9ff 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -136,7 +136,6 @@ void FlightModeManager::start_flight_task() { bool task_failure = false; bool should_disable_task = true; - int prev_failure_count = _task_failure_count; // Do not run any flight task for VTOLs in fixed-wing mode if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { @@ -146,23 +145,7 @@ void FlightModeManager::start_flight_task() // Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode) if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) { - - should_disable_task = false; - FlightTaskError error = switchTask(FlightTaskIndex::Transition); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Transition activation failed with error: %s", errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - + switchTask(FlightTaskIndex::Transition); return; } @@ -176,57 +159,24 @@ void FlightModeManager::start_flight_task() #endif // !CONSTRAINED_FLASH if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Follow-Me activation failed with error: %s", errorToString(error)); - } - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; } } else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { // Emergency descend should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - error = switchTask(FlightTaskIndex::Descend); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Descend activation failed with error: %s", errorToString(error)); - } + if (switchTask(FlightTaskIndex::Descend) != FlightTaskError::NoError) { task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; } } else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) { // Auto related maneuvers should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - error = switchTask(FlightTaskIndex::Auto); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Auto activation failed with error: %s", errorToString(error)); - } + if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) { task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; } } @@ -257,18 +207,7 @@ void FlightModeManager::start_flight_task() break; } - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Position-Ctrl activation failed with error: %s", errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL); - task_failure = false; - } + task_failure = error != FlightTaskError::NoError; } // manual altitude control @@ -287,18 +226,7 @@ void FlightModeManager::start_flight_task() break; } - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Altitude-Ctrl activation failed with error: %s", errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL); - task_failure = false; - } + task_failure = error != FlightTaskError::NoError; } if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) { @@ -307,6 +235,15 @@ void FlightModeManager::start_flight_task() // check task failure if (task_failure) { + // If failsafe task is activated for a prolonged time, print an error, as this is unexpected + if (_failsafe_task_activated_start == 0) { + _failsafe_task_activated_start = hrt_absolute_time(); + } + + if (hrt_absolute_time() - _failsafe_task_activated_start > 5_s && !_failsafe_task_error_printed) { + PX4_ERR("Failsafe task activated"); + _failsafe_task_error_printed = true; + } // for some reason no flighttask was able to start. // go into failsafe flighttask @@ -317,63 +254,15 @@ void FlightModeManager::start_flight_task() switchTask(FlightTaskIndex::None); } - } else if (should_disable_task) { - switchTask(FlightTaskIndex::None); + } else { + _failsafe_task_activated_start = 0; + _failsafe_task_error_printed = false; + + if (should_disable_task) { + switchTask(FlightTaskIndex::None); + } } - _last_vehicle_nav_state = _vehicle_status_sub.get().nav_state; -} - -void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state) -{ - if (!task_failure) { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - - } else if (_task_failure_count > NUM_FAILURE_TRIES) { - // tell commander to switch mode - PX4_WARN("Previous flight task failed, switching to mode %" PRIu8, nav_state); - send_vehicle_cmd_do(nav_state); - _task_failure_count = 0; // avoid immediate resending of a vehicle command in the next iteration - } -} - -void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state) -{ - vehicle_command_s command{}; - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode - command.target_system = 1; - command.target_component = 1; - command.source_system = 1; - command.source_component = 1; - command.confirmation = false; - command.from_external = false; - - // set the main mode - switch (nav_state) { - case vehicle_status_s::NAVIGATION_STATE_STAB: - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_STABILIZED; - break; - - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL; - break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - break; - - default: //vehicle_status_s::NAVIGATION_STATE_POSCTL - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL; - break; - } - - // publish the vehicle command - command.timestamp = hrt_absolute_time(); - _vehicle_command_pub.publish(command); } void FlightModeManager::handleCommand() diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 86dbc64913..74168d3e2f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -89,8 +89,6 @@ private: void Run() override; void updateParams() override; void start_flight_task(); - void check_failure(bool task_failure, uint8_t nav_state); - void send_vehicle_cmd_do(uint8_t nav_state); void handleCommand(); void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position); void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position); @@ -118,8 +116,6 @@ private: int _initTask(FlightTaskIndex task_index); FlightTaskIndex switchVehicleCommand(const int command); - static constexpr int NUM_FAILURE_TRIES = 10; ///< number of tries before switching to a failsafe flight task - /** * Union with all existing tasks: we use it to make sure that only the memory of the largest existing * task is needed, and to avoid using dynamic memory allocations. @@ -133,8 +129,9 @@ private: int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED}; - int _task_failure_count{0}; - uint8_t _last_vehicle_nav_state{0}; + + hrt_abstime _failsafe_task_activated_start{0}; + bool _failsafe_task_error_printed{false}; 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 @@ -153,7 +150,6 @@ private: uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; - uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};