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

FlightModeManager: rework task starting 

with the goal to make it more clear and the error only appear when
armed but every time the running task doesn't actually match
the mapping inside the start_flight_task() function.
This commit is contained in:
Matthias Grob 2023-04-12 15:50:54 +02:00
parent b02a785033
commit 1e94ad19c0
2 changed files with 64 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -136,9 +136,6 @@ void FlightModeManager::updateParams()
void FlightModeManager::start_flight_task()
{
bool task_failure = false;
bool should_disable_task = true;
// 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) {
switchTask(FlightTaskIndex::None);
@ -151,9 +148,14 @@ void FlightModeManager::start_flight_task()
return;
}
// Auto-follow me
bool found_some_task = false;
bool matching_task_running = true;
bool task_failure = false;
const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
// Follow me
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
should_disable_task = false;
found_some_task = true;
FlightTaskError error = FlightTaskError::InvalidTask;
#if !defined(CONSTRAINED_FLASH)
@ -161,31 +163,41 @@ void FlightModeManager::start_flight_task()
#endif // !CONSTRAINED_FLASH
if (error != FlightTaskError::NoError) {
matching_task_running = false;
task_failure = true;
}
} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
// Emergency descend
should_disable_task = false;
if (switchTask(FlightTaskIndex::Descend) != FlightTaskError::NoError) {
task_failure = true;
}
} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
// Auto related maneuvers
should_disable_task = false;
if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
task_failure = true;
}
}
// manual position control
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) {
should_disable_task = false;
// Orbit
if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)
&& !_command_failed) {
found_some_task = true;
FlightTaskError error = FlightTaskError::InvalidTask;
#if !defined(CONSTRAINED_FLASH)
error = switchTask(FlightTaskIndex::Orbit);
#endif // !CONSTRAINED_FLASH
if (error != FlightTaskError::NoError) {
matching_task_running = false;
task_failure = true;
}
}
// Navigator interface for autonomous modes
if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
&& !nav_state_descend) {
found_some_task = true;
if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
matching_task_running = false;
task_failure = true;
}
}
// Manual position control
if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) || task_failure) {
found_some_task = true;
FlightTaskError error = FlightTaskError::NoError;
switch (_param_mpc_pos_mode.get()) {
@ -209,12 +221,13 @@ void FlightModeManager::start_flight_task()
break;
}
task_failure = error != FlightTaskError::NoError;
task_failure = (error != FlightTaskError::NoError);
matching_task_running = matching_task_running && !task_failure;
}
// manual altitude control
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) {
should_disable_task = false;
// Manual altitude control
if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) || task_failure) {
found_some_task = true;
FlightTaskError error = FlightTaskError::NoError;
switch (_param_mpc_pos_mode.get()) {
@ -228,56 +241,35 @@ void FlightModeManager::start_flight_task()
break;
}
task_failure = error != FlightTaskError::NoError;
task_failure = (error != FlightTaskError::NoError);
matching_task_running = matching_task_running && !task_failure;
}
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
should_disable_task = false;
// Emergency descend
if (nav_state_descend || task_failure) {
found_some_task = true;
if (!_command_failed) {
FlightTaskError error = FlightTaskError::InvalidTask;
FlightTaskError error = switchTask(FlightTaskIndex::Descend);
#if !defined(CONSTRAINED_FLASH)
error = switchTask(FlightTaskIndex::Orbit);
#endif // !CONSTRAINED_FLASH
if (error != FlightTaskError::NoError) {
task_failure = true;
}
}
task_failure = (error != FlightTaskError::NoError);
matching_task_running = matching_task_running && !task_failure;
}
// 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
FlightTaskError error = switchTask(FlightTaskIndex::Failsafe);
if (error != FlightTaskError::NoError) {
// No task was activated.
switchTask(FlightTaskIndex::None);
}
} else {
_failsafe_task_activated_start = 0;
_failsafe_task_error_printed = false;
if (should_disable_task) {
switchTask(FlightTaskIndex::None);
}
// For some reason no task was able to start, go into failsafe flighttask
found_some_task = (switchTask(FlightTaskIndex::Failsafe) == FlightTaskError::NoError);
}
if (!found_some_task) {
switchTask(FlightTaskIndex::None);
}
if (!matching_task_running && _vehicle_control_mode_sub.get().flag_armed && !_no_matching_task_error_printed) {
PX4_ERR("Matching flight task was not able to run, Nav state: %" PRIu8 ", Task: %" PRIu32,
_vehicle_status_sub.get().nav_state, static_cast<uint32_t>(_current_task.index));
}
_no_matching_task_error_printed = !matching_task_running;
}
void FlightModeManager::tryApplyCommandIfAny()

3
src/modules/flight_mode_manager/FlightModeManager.hpp
View File

@ -130,8 +130,7 @@ private:
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED};
hrt_abstime _failsafe_task_activated_start{0};
bool _failsafe_task_error_printed{false};
bool _no_matching_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