fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 07:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MulticoperPositionController: time failsafe messages together

Browse Source 
The following output instead of printing the
action "stop and wait" just once:

WARN  [mc_pos_control] invalid setpoints
WARN  [mc_pos_control] invalid setpoints
WARN  [mc_pos_control] Failsafe: stop and wait
WARN  [mc_pos_control] invalid setpoints
WARN  [mc_pos_control] Failsafe: stop and wait
WARN  [mc_pos_control] invalid setpoints
WARN  [mc_pos_control] Failsafe: stop and wait
This commit is contained in:
Matthias Grob
2022-01-19 10:43:23 +01:00
parent d4e356a1ac
commit c9f7c20d46
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+4 -4
View File
@@ -321,8 +321,6 @@ void MulticopterPositionControl::Run()
// set _dt in controllib Block for BlockDerivative
setDt(dt);
const bool was_in_failsafe = _in_failsafe;
_in_failsafe = false;
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
@@ -478,14 +476,16 @@ void MulticopterPositionControl::Run()
} else {
// Failsafe
if ((time_stamp_now - _last_warn) > 2_s) {
const bool warn_failsafe = (time_stamp_now - _last_warn) > 2_s;
if (warn_failsafe) {
PX4_WARN("invalid setpoints");
_last_warn = time_stamp_now;
}
vehicle_local_position_setpoint_s failsafe_setpoint{};
failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe);
failsafe(time_stamp_now, failsafe_setpoint, states, warn_failsafe);
// reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}};