fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight_mode_manager: reduce error verbosity & remove mode switching

Browse Source 
While disarmed, commander allows to be in any mode now.
This commit is contained in:
Beat Küng 2022-09-03 10:32:11 +02:00 committed by Daniel Agar
parent 31dfdea12e
commit 57c7b5e843
2 changed files with 24 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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()

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

@ -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_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};